//

// Academic License - for use in teaching, academic research, and meeting

// course requirements at degree granting institutions only.  Not for

// government, commercial, or other organizational use.

//

// File: cartesian_waypoints_planner.cpp

//

// Code generated for Simulink model 'cartesian_waypoints_planner'.

//

// Model version                  : 1.189

// Simulink Coder version         : 9.3 (R2020a) 18-Nov-2019

// C/C++ source code generated on : Thu May 28 15:55:47 2020

//

// Target selection: ert.tlc

// Embedded hardware selection: Generic->Unspecified (assume 32-bit Generic)

// Code generation objectives: Unspecified

// Validation result: Not run

//

#include "cartesian_waypoints_planner.h"

#include "cartesian_waypoints_planner_private.h"



// Block signals (default storage)

B_cartesian_waypoints_planner_T cartesian_waypoints_planner_B;



// Block states (default storage)

DW_cartesian_waypoints_planne_T cartesian_waypoints_planner_DW;



// Real-time model

RT_MODEL_cartesian_waypoints__T cartesian_waypoints_planner_M_ =

  RT_MODEL_cartesian_waypoints__T();

RT_MODEL_cartesian_waypoints__T *const cartesian_waypoints_planner_M =

  &cartesian_waypoints_planner_M_;



// Forward declaration for local functions

static void car_quaternioncg_parenReference(real_T obj_a, real_T obj_b, real_T

  obj_c, real_T obj_d, real_T *o_a, real_T *o_b, real_T *o_c, real_T *o_d);

static void cartesian_waypoi_emxInit_real_T(emxArray_real_T_cartesian_way_T

  **pEmxArray, int32_T numDimensions);

static void cartes_emxEnsureCapacity_real_T(emxArray_real_T_cartesian_way_T

  *emxArray, int32_T oldNumel);

static void cartesian_waypoi_emxInit_char_T(emxArray_char_T_cartesian_way_T

  **pEmxArray, int32_T numDimensions);

static void cartes_emxEnsureCapacity_char_T(emxArray_char_T_cartesian_way_T

  *emxArray, int32_T oldNumel);

static void cartesian_waypoi_emxFree_real_T(emxArray_real_T_cartesian_way_T

  **pEmxArray);

static void cartesian_waypoi_emxFree_char_T(emxArray_char_T_cartesian_way_T

  **pEmxArray);

static void car_inverseKinematics_setupImpl(b_inverseKinematics_cartesian_T *obj,

  f_robotics_manip_internal_IKE_T *iobj_0);

static void c_inverseKinematics_setPoseGoal(b_inverseKinematics_cartesian_T *obj,

  const real_T tform[16], const real_T weights[6]);

static void RigidBodyTree_validateConfigu_e(x_robotics_manip_internal_Rig_T *obj,

  real_T Q[6]);

static boolean_T cartesian_waypoints_plan_strcmp(const

  emxArray_char_T_cartesian_way_T *a, const emxArray_char_T_cartesian_way_T *b);

static void ca_rigidBodyJoint_get_JointAxis(const

  c_rigidBodyJoint_cartesian_wa_T *obj, real_T ax[3]);

static void cartesian_waypoints_planner_cat(real_T varargin_1, real_T varargin_2,

  real_T varargin_3, real_T varargin_4, real_T varargin_5, real_T varargin_6,

  real_T varargin_7, real_T varargin_8, real_T varargin_9, real_T y[9]);

static void rigidBodyJoint_transformBodyT_e(const

  c_rigidBodyJoint_cartesian_wa_T *obj, const real_T q_data[], const int32_T

  *q_size, real_T T[16]);

static void rigidBodyJoint_transformBodyToP(const

  c_rigidBodyJoint_cartesian_wa_T *obj, real_T T[16]);

static void RigidBodyTree_efficientFKAndJac(x_robotics_manip_internal_Rig_T *obj,

  const real_T qv[6], const emxArray_char_T_cartesian_way_T *body1Name, real_T

  T_data[], int32_T T_size[2], emxArray_real_T_cartesian_way_T *Jac);

static real_T cartesian_waypoin_rt_hypotd_snf(real_T u0, real_T u1);

static creal_T cartesian_waypoints_planne_sqrt(const creal_T x);

static real_T cartesian_waypoints_plann_xnrm2(int32_T n, const real_T x[9],

  int32_T ix0);

static real_T cartesian_waypoints_plann_xdotc(int32_T n, const real_T x[9],

  int32_T ix0, const real_T y[9], int32_T iy0);

static void cartesian_waypoints_plann_xaxpy(int32_T n, real_T a, int32_T ix0,

  const real_T y[9], int32_T iy0, real_T b_y[9]);

static real_T cartesian_waypoints_pla_xnrm2_e(const real_T x[3], int32_T ix0);

static void cartesian_waypoints_p_xaxpy_evq(int32_T n, real_T a, const real_T x

  [9], int32_T ix0, real_T y[3], int32_T iy0);

static void cartesian_waypoints_pl_xaxpy_ev(int32_T n, real_T a, const real_T x

  [3], int32_T ix0, const real_T y[9], int32_T iy0, real_T b_y[9]);

static void cartesian_waypoints_plann_xswap(const real_T x[9], int32_T ix0,

  int32_T iy0, real_T b_x[9]);

static void cartesian_waypoints_plann_xrotg(real_T a, real_T b, real_T *b_a,

  real_T *b_b, real_T *c, real_T *s);

static void cartesian_waypoints_planne_xrot(const real_T x[9], int32_T ix0,

  int32_T iy0, real_T c, real_T s, real_T b_x[9]);

static void cartesian_waypoints_planner_svd(const real_T A[9], real_T U[9],

  real_T s[3], real_T V[9]);

static void cartesian_waypoints__rotm2axang(const real_T R[9], real_T axang[4]);

static void cartesian_IKHelpers_computeCost(const real_T x[6],

  f_robotics_manip_internal_IKE_T *args, real_T *cost, real_T W[36],

  emxArray_real_T_cartesian_way_T *Jac, f_robotics_manip_internal_IKE_T **b_args);

static void cartesian_waypoints_planner_eye(real_T b_I[36]);

static void cartesian_way_emxInit_boolean_T(emxArray_boolean_T_cartesian__T

  **pEmxArray, int32_T numDimensions);

static void cartesian_waypo_emxInit_int32_T(emxArray_int32_T_cartesian_wa_T

  **pEmxArray, int32_T numDimensions);

static void car_emxEnsureCapacity_boolean_T(emxArray_boolean_T_cartesian__T

  *emxArray, int32_T oldNumel);

static void carte_emxEnsureCapacity_int32_T(emxArray_int32_T_cartesian_wa_T

  *emxArray, int32_T oldNumel);

static real_T cartesian_waypoints_plan_norm_e(const real_T x[6]);

static real_T SystemTimeProvider_getElapsedTi(const

  f_robotics_core_internal_Syst_T *obj);

static real_T cartesian_waypoints_pl_xnrm2_ev(int32_T n, const

  emxArray_real_T_cartesian_way_T *x, int32_T ix0);

static void cartesian_waypoints_plan_qrpf_e(const

  emxArray_real_T_cartesian_way_T *A, int32_T m, int32_T n,

  emxArray_real_T_cartesian_way_T *tau, const emxArray_int32_T_cartesian_wa_T

  *jpvt, emxArray_real_T_cartesian_way_T *b_A, emxArray_int32_T_cartesian_wa_T

  *b_jpvt);

static void cartesian_waypoints_pla_xzgetrf(int32_T m, int32_T n, const

  emxArray_real_T_cartesian_way_T *A, int32_T lda,

  emxArray_real_T_cartesian_way_T *b_A, emxArray_int32_T_cartesian_wa_T *ipiv,

  int32_T *info);

static void cartesian_waypoints_plann_xtrsm(int32_T m, int32_T n, const

  emxArray_real_T_cartesian_way_T *A, int32_T lda, const

  emxArray_real_T_cartesian_way_T *B, int32_T ldb,

  emxArray_real_T_cartesian_way_T *b_B);

static void cartesian_waypo_emxFree_int32_T(emxArray_int32_T_cartesian_wa_T

  **pEmxArray);

static void cartesian_waypoints_pl_mldivide(const

  emxArray_real_T_cartesian_way_T *A, const emxArray_real_T_cartesian_way_T *B,

  emxArray_real_T_cartesian_way_T *Y);

static void cartesian_way_emxFree_boolean_T(emxArray_boolean_T_cartesian__T

  **pEmxArray);

static boolean_T DampedBFGSwGradientProjection_a(const

  h_robotics_core_internal_Damp_T *obj, const real_T Hg[6], const

  emxArray_real_T_cartesian_way_T *alpha);

static void cartesian_waypoints_planner_inv(const

  emxArray_real_T_cartesian_way_T *x, emxArray_real_T_cartesian_way_T *y);

static void cartesian_waypoints_planne_diag(const

  emxArray_real_T_cartesian_way_T *v, emxArray_real_T_cartesian_way_T *d);

static void cartesian_waypoints_pla_sqrt_ev(emxArray_real_T_cartesian_way_T *x);

static boolean_T cartesian_waypoints_planner_any(const

  emxArray_boolean_T_cartesian__T *x);

static boolean_T cartesian_wa_isPositiveDefinite(const real_T B[36]);

static boolean_T DampedBFGSwGradientProjection_e(const

  h_robotics_core_internal_Damp_T *obj, const real_T xNew[6]);

static void DampedBFGSwGradientProjection_s(h_robotics_core_internal_Damp_T *obj,

  real_T xSol[6], c_robotics_core_internal_NLPS_T *exitFlag, real_T *err, real_T

  *iter);

static void cartesian_waypoints_pl_isfinite(const

  emxArray_real_T_cartesian_way_T *x, emxArray_boolean_T_cartesian__T *b);

static void cartesi_genrand_uint32_vector_e(uint32_T mt[625], uint32_T u[2]);

static boolean_T cartesian_waypoi_is_valid_state(const uint32_T mt[625]);

static real_T cartesian_waypoints__genrandu_e(uint32_T mt[625]);

static real_T cartesia_eml_rand_mt19937ar_evq(uint32_T state[625]);

static void cartesian_waypoints_plann_randn(const real_T varargin_1[2],

  emxArray_real_T_cartesian_way_T *r);

static void cartesian__eml_rand_mt19937ar_e(const uint32_T state[625], uint32_T

  b_state[625], real_T *r);

static void cartesian_waypoints_plan_rand_e(real_T varargin_1,

  emxArray_real_T_cartesian_way_T *r);

static void cartes_NLPSolverInterface_solve(h_robotics_core_internal_Damp_T *obj,

  const real_T seed[6], real_T xSol[6], real_T *solutionInfo_Iterations, real_T *

  solutionInfo_RRAttempts, real_T *solutionInfo_Error, real_T

  *solutionInfo_ExitFlag, char_T solutionInfo_Status_data[], int32_T

  solutionInfo_Status_size[2]);

static void cart_inverseKinematics_stepImpl(b_inverseKinematics_cartesian_T *obj,

  const real_T tform[16], const real_T weights[6], const real_T initialGuess[6],

  real_T QSol[6]);

static void cartesian_w_emxInit_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T

  **pEmxArray, int32_T numDimensions);

static void c_emxEnsureCapacity_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T

  *emxArray, int32_T oldNumel);

static void rigidBodyJoint_get_JointAxis_e(const c_rigidBodyJoint_cartesian__e_T

  *obj, real_T ax[3]);

static void RigidBodyTree_forwardKinematics(p_robotics_manip_internal_Rig_T *obj,

  const real_T qvec[6], emxArray_f_cell_wrap_cartesia_T *Ttree);

static void cartesian_w_emxFree_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T

  **pEmxArray);

static boolean_T cartesian_waypoint_anyNonFinite(const real_T x[16]);

static real_T cartesian_waypoints_pl_xzlangeM(const creal_T x[16]);

static void cartesian_waypoints_pla_xzlascl(real_T cfrom, real_T cto, creal_T A

  [16]);

static real_T cartesian_waypoints_pla_xzlanhs(const creal_T A[16], int32_T ilo,

  int32_T ihi);

static void cartesian_waypoints_p_xzlartg_i(const creal_T f, const creal_T g,

  real_T *cs, creal_T *sn);

static void cartesian_waypoints_pla_xzlartg(const creal_T f, const creal_T g,

  real_T *cs, creal_T *sn, creal_T *r);

static void cartesian_waypoints_pla_xzhgeqz(creal_T A[16], int32_T ilo, int32_T

  ihi, creal_T Z[16], int32_T *info, creal_T alpha1[4], creal_T beta1[4]);

static void cartesian_waypoints_pla_xztgevc(const creal_T A[16], creal_T V[16]);

static void cartesian_waypoints_plan_xzggev(creal_T A[16], int32_T *info,

  creal_T alpha1[4], creal_T beta1[4], creal_T V[16]);

static real_T cartesian_waypoints_pla_xnrm2_c(int32_T n, const real_T x[16],

  int32_T ix0);

static void cartesian_waypoints_plan_xzlarf(int32_T m, int32_T n, int32_T iv0,

  real_T tau, real_T C[16], int32_T ic0, real_T work[4]);

static void cartesian_waypoints_plan_xgehrd(real_T a[16], real_T tau[3]);

static real_T cartesian_waypoints_pl_xnrm2_cc(int32_T n, const real_T x[3]);

static real_T cartesian_waypoints_pla_xzlarfg(int32_T n, real_T *alpha1, real_T

  x[3]);

static void cartesian_waypoints_pla_xdlanv2(real_T *a, real_T *b, real_T *c,

  real_T *d, real_T *rt1r, real_T *rt1i, real_T *rt2r, real_T *rt2i, real_T *cs,

  real_T *sn);

static void cartesian_waypoints_plan_xrot_l(int32_T n, real_T x[16], int32_T ix0,

  int32_T iy0, real_T c, real_T s);

static void cartesian_waypoints_pla_xrot_lr(int32_T n, real_T x[16], int32_T ix0,

  int32_T iy0, real_T c, real_T s);

static int32_T cartesian_waypoints__eml_dlahqr(real_T h[16], real_T z[16]);

static void cartesian_waypoints_planner_eig(const real_T A[16], creal_T V[16],

  creal_T D[4]);

static void matlabCodegenHandle_matlabCodeg(ros_slros_internal_block_GetP_T *obj);

static void matlabCodegenHandle_matlab_evqu(robotics_slmanip_internal_blo_T *obj);

static void cartesian_wa_SystemCore_release(b_inverseKinematics_cartesian_T *obj);

static void cartesian_SystemCore_delete_evq(b_inverseKinematics_cartesian_T *obj);

static void matlabCodegenHandle_matlabC_evq(b_inverseKinematics_cartesian_T *obj);

static void emxFreeStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian_wa_T

  *pStruct);

static void emxFreeStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T

  *pStruct);

static void emxFreeStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T

  *pStruct);

static void emxFreeStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T

  *pStruct);

static void emxFreeStruct_robotics_slmanip_(robotics_slmanip_internal_blo_T

  *pStruct);

static void emxFreeStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T

  *pStruct);

static void emxFreeStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T

  *pStruct);

static void emxFreeStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T

  *pStruct);

static void emxFreeStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T

  *pStruct);

static void matlabCodegenHandle_mat_evqusng(ros_slros_internal_block_Subs_T *obj);

static void emxFreeStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian__e_T

  *pStruct);

static void emxFreeStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T

  *pStruct);

static void emxFreeStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T

  *pStruct);

static void emxFreeStruct_robotics_slmani_e(robotics_slmanip_internal_b_e_T

  *pStruct);

static void emxFreeStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T

  *pStruct);

static void cartesian_wa_matlabCodegenHa_lb(ros_slros_internal_block_SetP_T *obj);

static void matlabCodegenHandle_matla_evqus(ros_slros_internal_block_Publ_T *obj);

static void emxInitStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian_wa_T

  *pStruct);

static void emxInitStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T

  *pStruct);

static void emxInitStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T

  *pStruct);

static void emxInitStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T

  *pStruct);

static void emxInitStruct_robotics_slmanip_(robotics_slmanip_internal_blo_T

  *pStruct);

static void emxInitStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T

  *pStruct);

static void emxInitStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T

  *pStruct);

static void emxInitStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T

  *pStruct);

static void emxInitStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T

  *pStruct);

static void cartesia_twister_state_vector_e(uint32_T mt[625]);

static void cartesian_wa_eml_rand_mt19937ar(uint32_T state[625]);

static v_robotics_manip_internal_Rig_T *cartesian_w_RigidBody_RigidBody

  (v_robotics_manip_internal_Rig_T *obj);

static v_robotics_manip_internal_Rig_T *cartesian_RigidBody_RigidBody_e

  (v_robotics_manip_internal_Rig_T *obj);

static v_robotics_manip_internal_Rig_T *cartesia_RigidBody_RigidBody_ev

  (v_robotics_manip_internal_Rig_T *obj);

static v_robotics_manip_internal_Rig_T *cartesi_RigidBody_RigidBody_evq

  (v_robotics_manip_internal_Rig_T *obj);

static v_robotics_manip_internal_Rig_T *cartes_RigidBody_RigidBody_evqu

  (v_robotics_manip_internal_Rig_T *obj);

static v_robotics_manip_internal_Rig_T *carte_RigidBody_RigidBody_evqus

  (v_robotics_manip_internal_Rig_T *obj);

static v_robotics_manip_internal_Rig_T *cart_RigidBody_RigidBody_evqusn

  (v_robotics_manip_internal_Rig_T *obj);

static y_robotics_manip_internal_Rig_T *car_RigidBodyTree_RigidBodyTree

  (y_robotics_manip_internal_Rig_T *obj, v_robotics_manip_internal_Rig_T *iobj_0,

   v_robotics_manip_internal_Rig_T *iobj_1, v_robotics_manip_internal_Rig_T

   *iobj_2, v_robotics_manip_internal_Rig_T *iobj_3,

   v_robotics_manip_internal_Rig_T *iobj_4, v_robotics_manip_internal_Rig_T

   *iobj_5, v_robotics_manip_internal_Rig_T *iobj_6,

   v_robotics_manip_internal_Rig_T *iobj_7);

static void cartesian_waypoints_planne_rand(real_T r[5]);

static w_robotics_manip_internal_Rig_T *car_RigidBody_RigidBody_evqusng

  (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0);

static w_robotics_manip_internal_Rig_T *ca_RigidBody_RigidBody_evqusngv

  (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0);

static w_robotics_manip_internal_Rig_T *c_RigidBody_RigidBody_evqusngvo

  (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0);

static w_robotics_manip_internal_Rig_T *RigidBody_RigidBody_evqusngvo0

  (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0);

static w_robotics_manip_internal_Rig_T *RigidBody_RigidBody_evqusngvo0a

  (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0);

static void ca_RigidBodyTree_clearAllBodies(x_robotics_manip_internal_Rig_T *obj,

  w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T

  *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,

  w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T

  *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,

  w_robotics_manip_internal_Rig_T *iobj_6, c_rigidBodyJoint_cartesian_wa_T

  *iobj_7, c_rigidBodyJoint_cartesian_wa_T *iobj_8,

  c_rigidBodyJoint_cartesian_wa_T *iobj_9, c_rigidBodyJoint_cartesian_wa_T

  *iobj_10, c_rigidBodyJoint_cartesian_wa_T *iobj_11,

  c_rigidBodyJoint_cartesian_wa_T *iobj_12, c_rigidBodyJoint_cartesian_wa_T

  *iobj_13, c_rigidBodyJoint_cartesian_wa_T *iobj_14,

  w_robotics_manip_internal_Rig_T *iobj_15);

static x_robotics_manip_internal_Rig_T *c_RigidBodyTree_RigidBodyTree_e

  (x_robotics_manip_internal_Rig_T *obj, w_robotics_manip_internal_Rig_T *iobj_0,

   w_robotics_manip_internal_Rig_T *iobj_1, w_robotics_manip_internal_Rig_T

   *iobj_2, w_robotics_manip_internal_Rig_T *iobj_3,

   w_robotics_manip_internal_Rig_T *iobj_4, w_robotics_manip_internal_Rig_T

   *iobj_5, w_robotics_manip_internal_Rig_T *iobj_6,

   c_rigidBodyJoint_cartesian_wa_T *iobj_7, c_rigidBodyJoint_cartesian_wa_T

   *iobj_8, c_rigidBodyJoint_cartesian_wa_T *iobj_9,

   c_rigidBodyJoint_cartesian_wa_T *iobj_10, c_rigidBodyJoint_cartesian_wa_T

   *iobj_11, c_rigidBodyJoint_cartesian_wa_T *iobj_12,

   c_rigidBodyJoint_cartesian_wa_T *iobj_13, c_rigidBodyJoint_cartesian_wa_T

   *iobj_14, c_rigidBodyJoint_cartesian_wa_T *iobj_15,

   w_robotics_manip_internal_Rig_T *iobj_16);

static c_rigidBodyJoint_cartesian_wa_T *c_rigidBodyJoint_rigidBodyJoint

  (c_rigidBodyJoint_cartesian_wa_T *obj, const emxArray_char_T_cartesian_way_T

   *jname, const emxArray_char_T_cartesian_way_T *jtype);

static w_robotics_manip_internal_Rig_T *cartesian_waypoi_RigidBody_copy(const

  v_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0,

  c_rigidBodyJoint_cartesian_wa_T *iobj_1, w_robotics_manip_internal_Rig_T

  *iobj_2);

static void cartesian_RigidBodyTree_addBody(x_robotics_manip_internal_Rig_T *obj,

  v_robotics_manip_internal_Rig_T *bodyin, const emxArray_char_T_cartesian_way_T

  *parentName, c_rigidBodyJoint_cartesian_wa_T *iobj_0,

  c_rigidBodyJoint_cartesian_wa_T *iobj_1, w_robotics_manip_internal_Rig_T

  *iobj_2);

static void inverseKinematics_set_RigidBody(b_inverseKinematics_cartesian_T *obj,

  y_robotics_manip_internal_Rig_T *rigidbodytree,

  w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T

  *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,

  w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T

  *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,

  w_robotics_manip_internal_Rig_T *iobj_6, w_robotics_manip_internal_Rig_T

  *iobj_7, w_robotics_manip_internal_Rig_T *iobj_8,

  w_robotics_manip_internal_Rig_T *iobj_9, w_robotics_manip_internal_Rig_T

  *iobj_10, w_robotics_manip_internal_Rig_T *iobj_11,

  w_robotics_manip_internal_Rig_T *iobj_12, w_robotics_manip_internal_Rig_T

  *iobj_13, w_robotics_manip_internal_Rig_T *iobj_14,

  c_rigidBodyJoint_cartesian_wa_T *iobj_15, c_rigidBodyJoint_cartesian_wa_T

  *iobj_16, c_rigidBodyJoint_cartesian_wa_T *iobj_17,

  c_rigidBodyJoint_cartesian_wa_T *iobj_18, c_rigidBodyJoint_cartesian_wa_T

  *iobj_19, c_rigidBodyJoint_cartesian_wa_T *iobj_20,

  c_rigidBodyJoint_cartesian_wa_T *iobj_21, c_rigidBodyJoint_cartesian_wa_T

  *iobj_22, c_rigidBodyJoint_cartesian_wa_T *iobj_23,

  c_rigidBodyJoint_cartesian_wa_T *iobj_24, c_rigidBodyJoint_cartesian_wa_T

  *iobj_25, c_rigidBodyJoint_cartesian_wa_T *iobj_26,

  c_rigidBodyJoint_cartesian_wa_T *iobj_27, c_rigidBodyJoint_cartesian_wa_T

  *iobj_28, c_rigidBodyJoint_cartesian_wa_T *iobj_29,

  c_rigidBodyJoint_cartesian_wa_T *iobj_30, c_rigidBodyJoint_cartesian_wa_T

  *iobj_31, c_rigidBodyJoint_cartesian_wa_T *iobj_32,

  c_rigidBodyJoint_cartesian_wa_T *iobj_33, c_rigidBodyJoint_cartesian_wa_T

  *iobj_34, c_rigidBodyJoint_cartesian_wa_T *iobj_35,

  c_rigidBodyJoint_cartesian_wa_T *iobj_36, c_rigidBodyJoint_cartesian_wa_T

  *iobj_37, c_rigidBodyJoint_cartesian_wa_T *iobj_38,

  c_rigidBodyJoint_cartesian_wa_T *iobj_39, w_robotics_manip_internal_Rig_T

  *iobj_40, x_robotics_manip_internal_Rig_T *iobj_41);

static h_robotics_core_internal_Damp_T *DampedBFGSwGradientProjection_D

  (h_robotics_core_internal_Damp_T *obj);

static void emxInitStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian__e_T

  *pStruct);

static void emxInitStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T

  *pStruct);

static void emxInitStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T

  *pStruct);

static void emxInitStruct_robotics_slmani_e(robotics_slmanip_internal_b_e_T

  *pStruct);

static void emxInitStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T

  *pStruct);

static n_robotics_manip_internal_Rig_T *RigidBody_RigidBod_evqusngvo0ar

  (n_robotics_manip_internal_Rig_T *obj);

static n_robotics_manip_internal_Rig_T *RigidBody_RigidBo_evqusngvo0ar1

  (n_robotics_manip_internal_Rig_T *obj);

static n_robotics_manip_internal_Rig_T *RigidBody_RigidB_evqusngvo0ar1d

  (n_robotics_manip_internal_Rig_T *obj);

static n_robotics_manip_internal_Rig_T *RigidBody_Rigid_evqusngvo0ar1db

  (n_robotics_manip_internal_Rig_T *obj);

static n_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_c

  (n_robotics_manip_internal_Rig_T *obj);

static p_robotics_manip_internal_Rig_T *RigidBodyTree_RigidBodyTree_ev

  (p_robotics_manip_internal_Rig_T *obj, n_robotics_manip_internal_Rig_T *iobj_0,

   n_robotics_manip_internal_Rig_T *iobj_1, n_robotics_manip_internal_Rig_T

   *iobj_2, n_robotics_manip_internal_Rig_T *iobj_3,

   n_robotics_manip_internal_Rig_T *iobj_4, n_robotics_manip_internal_Rig_T

   *iobj_5, n_robotics_manip_internal_Rig_T *iobj_6,

   n_robotics_manip_internal_Rig_T *iobj_7);

int32_T div_s32_floor(int32_T numerator, int32_T denominator)

{

  int32_T quotient;

  uint32_T absNumerator;

  uint32_T absDenominator;

  uint32_T tempAbsQuotient;

  boolean_T quotientNeedsNegation;

  if (denominator == 0) {

    quotient = numerator >= 0 ? MAX_int32_T : MIN_int32_T;



    // Divide by zero handler

  } else {

    absNumerator = numerator < 0 ? ~static_cast<uint32_T>(numerator) + 1U :

      static_cast<uint32_T>(numerator);

    absDenominator = denominator < 0 ? ~static_cast<uint32_T>(denominator) + 1U :

      static_cast<uint32_T>(denominator);

    quotientNeedsNegation = ((numerator < 0) != (denominator < 0));

    tempAbsQuotient = absNumerator / absDenominator;

    if (quotientNeedsNegation) {

      absNumerator %= absDenominator;

      if (absNumerator > 0U) {

        tempAbsQuotient++;

      }

    }



    quotient = quotientNeedsNegation ? -static_cast<int32_T>(tempAbsQuotient) :

      static_cast<int32_T>(tempAbsQuotient);

  }



  return quotient;

}



int32_T div_nzp_s32(int32_T numerator, int32_T denominator)

{

  uint32_T tempAbsQuotient;

  tempAbsQuotient = (numerator < 0 ? ~static_cast<uint32_T>(numerator) + 1U :

                     static_cast<uint32_T>(numerator)) / (denominator < 0 ? ~

    static_cast<uint32_T>(denominator) + 1U : static_cast<uint32_T>(denominator));

  return (numerator < 0) != (denominator < 0) ? -static_cast<int32_T>

    (tempAbsQuotient) : static_cast<int32_T>(tempAbsQuotient);

}



// Function for MATLAB Function: '<Root>/MATLAB Function'

static void car_quaternioncg_parenReference(real_T obj_a, real_T obj_b, real_T

  obj_c, real_T obj_d, real_T *o_a, real_T *o_b, real_T *o_c, real_T *o_d)

{

  *o_a = obj_a;

  *o_b = obj_b;

  *o_c = obj_c;

  *o_d = obj_d;

}



static void cartesian_waypoi_emxInit_real_T(emxArray_real_T_cartesian_way_T

  **pEmxArray, int32_T numDimensions)

{

  emxArray_real_T_cartesian_way_T *emxArray;

  *pEmxArray = (emxArray_real_T_cartesian_way_T *)malloc(sizeof

    (emxArray_real_T_cartesian_way_T));

  emxArray = *pEmxArray;

  emxArray->data = (real_T *)NULL;

  emxArray->numDimensions = numDimensions;

  emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);

  emxArray->allocatedSize = 0;

  emxArray->canFreeData = true;

  for (cartesian_waypoints_planner_B.i_ph = 0;

       cartesian_waypoints_planner_B.i_ph < numDimensions;

       cartesian_waypoints_planner_B.i_ph++) {

    emxArray->size[cartesian_waypoints_planner_B.i_ph] = 0;

  }

}



static void cartes_emxEnsureCapacity_real_T(emxArray_real_T_cartesian_way_T

  *emxArray, int32_T oldNumel)

{

  void *newData;

  if (oldNumel < 0) {

    oldNumel = 0;

  }



  cartesian_waypoints_planner_B.newNumel_g = 1;

  for (cartesian_waypoints_planner_B.i_o = 0; cartesian_waypoints_planner_B.i_o <

       emxArray->numDimensions; cartesian_waypoints_planner_B.i_o++) {

    cartesian_waypoints_planner_B.newNumel_g *= emxArray->

      size[cartesian_waypoints_planner_B.i_o];

  }



  if (cartesian_waypoints_planner_B.newNumel_g > emxArray->allocatedSize) {

    cartesian_waypoints_planner_B.i_o = emxArray->allocatedSize;

    if (cartesian_waypoints_planner_B.i_o < 16) {

      cartesian_waypoints_planner_B.i_o = 16;

    }



    while (cartesian_waypoints_planner_B.i_o <

           cartesian_waypoints_planner_B.newNumel_g) {

      if (cartesian_waypoints_planner_B.i_o > 1073741823) {

        cartesian_waypoints_planner_B.i_o = MAX_int32_T;

      } else {

        cartesian_waypoints_planner_B.i_o <<= 1;

      }

    }



    newData = calloc(static_cast<uint32_T>(cartesian_waypoints_planner_B.i_o),

                     sizeof(real_T));

    if (emxArray->data != NULL) {

      memcpy(newData, emxArray->data, sizeof(real_T) * oldNumel);

      if (emxArray->canFreeData) {

        free(emxArray->data);

      }

    }



    emxArray->data = (real_T *)newData;

    emxArray->allocatedSize = cartesian_waypoints_planner_B.i_o;

    emxArray->canFreeData = true;

  }

}



static void cartesian_waypoi_emxInit_char_T(emxArray_char_T_cartesian_way_T

  **pEmxArray, int32_T numDimensions)

{

  emxArray_char_T_cartesian_way_T *emxArray;

  *pEmxArray = (emxArray_char_T_cartesian_way_T *)malloc(sizeof

    (emxArray_char_T_cartesian_way_T));

  emxArray = *pEmxArray;

  emxArray->data = (char_T *)NULL;

  emxArray->numDimensions = numDimensions;

  emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);

  emxArray->allocatedSize = 0;

  emxArray->canFreeData = true;

  for (cartesian_waypoints_planner_B.i_g = 0; cartesian_waypoints_planner_B.i_g <

       numDimensions; cartesian_waypoints_planner_B.i_g++) {

    emxArray->size[cartesian_waypoints_planner_B.i_g] = 0;

  }

}



static void cartes_emxEnsureCapacity_char_T(emxArray_char_T_cartesian_way_T

  *emxArray, int32_T oldNumel)

{

  void *newData;

  if (oldNumel < 0) {

    oldNumel = 0;

  }



  cartesian_waypoints_planner_B.newNumel = 1;

  for (cartesian_waypoints_planner_B.i_k = 0; cartesian_waypoints_planner_B.i_k <

       emxArray->numDimensions; cartesian_waypoints_planner_B.i_k++) {

    cartesian_waypoints_planner_B.newNumel *= emxArray->

      size[cartesian_waypoints_planner_B.i_k];

  }



  if (cartesian_waypoints_planner_B.newNumel > emxArray->allocatedSize) {

    cartesian_waypoints_planner_B.i_k = emxArray->allocatedSize;

    if (cartesian_waypoints_planner_B.i_k < 16) {

      cartesian_waypoints_planner_B.i_k = 16;

    }



    while (cartesian_waypoints_planner_B.i_k <

           cartesian_waypoints_planner_B.newNumel) {

      if (cartesian_waypoints_planner_B.i_k > 1073741823) {

        cartesian_waypoints_planner_B.i_k = MAX_int32_T;

      } else {

        cartesian_waypoints_planner_B.i_k <<= 1;

      }

    }



    newData = calloc(static_cast<uint32_T>(cartesian_waypoints_planner_B.i_k),

                     sizeof(char_T));

    if (emxArray->data != NULL) {

      memcpy(newData, emxArray->data, sizeof(char_T) * oldNumel);

      if (emxArray->canFreeData) {

        free(emxArray->data);

      }

    }



    emxArray->data = (char_T *)newData;

    emxArray->allocatedSize = cartesian_waypoints_planner_B.i_k;

    emxArray->canFreeData = true;

  }

}



static void cartesian_waypoi_emxFree_real_T(emxArray_real_T_cartesian_way_T

  **pEmxArray)

{

  if (*pEmxArray != (emxArray_real_T_cartesian_way_T *)NULL) {

    if (((*pEmxArray)->data != (real_T *)NULL) && (*pEmxArray)->canFreeData) {

      free((*pEmxArray)->data);

    }



    free((*pEmxArray)->size);

    free(*pEmxArray);

    *pEmxArray = (emxArray_real_T_cartesian_way_T *)NULL;

  }

}



static void cartesian_waypoi_emxFree_char_T(emxArray_char_T_cartesian_way_T

  **pEmxArray)

{

  if (*pEmxArray != (emxArray_char_T_cartesian_way_T *)NULL) {

    if (((*pEmxArray)->data != (char_T *)NULL) && (*pEmxArray)->canFreeData) {

      free((*pEmxArray)->data);

    }



    free((*pEmxArray)->size);

    free(*pEmxArray);

    *pEmxArray = (emxArray_char_T_cartesian_way_T *)NULL;

  }

}



static void car_inverseKinematics_setupImpl(b_inverseKinematics_cartesian_T *obj,

  f_robotics_manip_internal_IKE_T *iobj_0)

{

  real_T n;

  emxArray_real_T_cartesian_way_T *A;

  emxArray_real_T_cartesian_way_T *b;

  real_T m;

  c_rigidBodyJoint_cartesian_wa_T *joint;

  real_T pnum;

  int32_T d;

  int32_T b_i;

  emxArray_real_T_cartesian_way_T *e;

  int32_T j;

  int32_T p;

  emxArray_real_T_cartesian_way_T *s;

  x_robotics_manip_internal_Rig_T *obj_0;

  w_robotics_manip_internal_Rig_T *body;

  int32_T c;

  boolean_T b_bool;

  emxArray_char_T_cartesian_way_T *a;

  int32_T b_kstr;

  char_T b_0[5];

  int8_T b_I[16];

  int32_T m_0;

  real_T t;

  real_T pnum_0;

  int32_T loop_ub;

  real_T tmp;

  real_T w;

  static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };



  int32_T exitg1;

  cartesian_waypoi_emxInit_real_T(&A, 2);

  n = obj->RigidBodyTreeInternal->PositionNumber;

  b_kstr = A->size[0] * A->size[1];

  c = static_cast<int32_T>(n);

  A->size[0] = c;

  loop_ub = static_cast<int32_T>(2.0 * n);

  A->size[1] = loop_ub;

  cartes_emxEnsureCapacity_real_T(A, b_kstr);

  m_0 = loop_ub * c - 1;

  for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {

    A->data[b_kstr] = 0.0;

  }



  cartesian_waypoi_emxInit_real_T(&b, 1);

  b_kstr = b->size[0];

  b->size[0] = loop_ub;

  cartes_emxEnsureCapacity_real_T(b, b_kstr);

  for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {

    b->data[b_kstr] = 0.0;

  }



  n = 1.0;

  m = 1.0;

  pnum = obj->RigidBodyTreeInternal->NumBodies;

  d = static_cast<int32_T>(pnum) - 1;

  cartesian_waypoi_emxInit_real_T(&e, 2);

  cartesian_waypoi_emxInit_real_T(&s, 2);

  cartesian_waypoi_emxInit_char_T(&a, 2);

  if (0 <= d) {

    for (b_kstr = 0; b_kstr < 5; b_kstr++) {

      b_0[b_kstr] = tmp_0[b_kstr];

    }

  }



  for (b_i = 0; b_i <= d; b_i++) {

    body = obj->RigidBodyTreeInternal->Bodies[b_i];

    joint = body->JointInternal;

    pnum = joint->PositionNumber;

    b_kstr = a->size[0] * a->size[1];

    a->size[0] = 1;

    a->size[1] = joint->Type->size[1];

    cartes_emxEnsureCapacity_char_T(a, b_kstr);

    loop_ub = joint->Type->size[0] * joint->Type->size[1] - 1;

    for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

      a->data[b_kstr] = joint->Type->data[b_kstr];

    }



    b_bool = false;

    if (a->size[1] == 5) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 5) {

          loop_ub = b_kstr - 1;

          if (a->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (!b_bool) {

      tmp = (n + pnum) - 1.0;

      if (n > tmp) {

        j = 0;

      } else {

        j = static_cast<int32_T>(n) - 1;

      }



      w = m + pnum;

      if (m > w - 1.0) {

        p = 0;

      } else {

        p = static_cast<int32_T>(m) - 1;

      }



      if (pnum < 0.0) {

        t = 0.0;

        pnum_0 = 0.0;

      } else {

        t = pnum;

        pnum_0 = pnum;

      }



      m_0 = static_cast<int32_T>(pnum_0) - 1;

      b_kstr = s->size[0] * s->size[1];

      c = static_cast<int32_T>(t);

      s->size[0] = c;

      s->size[1] = c;

      cartes_emxEnsureCapacity_real_T(s, b_kstr);

      loop_ub = c * c - 1;

      for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

        s->data[b_kstr] = 0.0;

      }



      if (c > 0) {

        for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {

          s->data[b_kstr + s->size[0] * b_kstr] = 1.0;

        }

      }



      loop_ub = s->size[1];

      for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {

        m_0 = s->size[0];

        for (c = 0; c < m_0; c++) {

          A->data[(j + c) + A->size[0] * (p + b_kstr)] = s->data[s->size[0] *

            b_kstr + c];

        }

      }



      if (n > tmp) {

        j = 0;

      } else {

        j = static_cast<int32_T>(n) - 1;

      }



      tmp = 2.0 * pnum + m;

      if (w > tmp - 1.0) {

        p = 0;

      } else {

        p = static_cast<int32_T>(w) - 1;

      }



      if (pnum < 0.0) {

        t = 0.0;

        pnum_0 = 0.0;

      } else {

        t = pnum;

        pnum_0 = pnum;

      }



      m_0 = static_cast<int32_T>(pnum_0) - 1;

      b_kstr = s->size[0] * s->size[1];

      c = static_cast<int32_T>(t);

      s->size[0] = c;

      s->size[1] = c;

      cartes_emxEnsureCapacity_real_T(s, b_kstr);

      loop_ub = c * c - 1;

      for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

        s->data[b_kstr] = 0.0;

      }



      if (c > 0) {

        for (b_kstr = 0; b_kstr <= m_0; b_kstr++) {

          s->data[b_kstr + s->size[0] * b_kstr] = 1.0;

        }

      }



      loop_ub = s->size[1];

      for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {

        m_0 = s->size[0];

        for (c = 0; c < m_0; c++) {

          A->data[(j + c) + A->size[0] * (p + b_kstr)] = -s->data[s->size[0] *

            b_kstr + c];

        }

      }



      b_kstr = e->size[0] * e->size[1];

      e->size[0] = joint->PositionLimitsInternal->size[0];

      e->size[1] = 2;

      cartes_emxEnsureCapacity_real_T(e, b_kstr);

      loop_ub = joint->PositionLimitsInternal->size[0] *

        joint->PositionLimitsInternal->size[1] - 1;

      for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

        e->data[b_kstr] = joint->PositionLimitsInternal->data[b_kstr];

      }



      b->data[static_cast<int32_T>(m) - 1] = e->data[1];

      b_kstr = e->size[0] * e->size[1];

      e->size[0] = joint->PositionLimitsInternal->size[0];

      e->size[1] = 2;

      cartes_emxEnsureCapacity_real_T(e, b_kstr);

      loop_ub = joint->PositionLimitsInternal->size[0] *

        joint->PositionLimitsInternal->size[1] - 1;

      for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

        e->data[b_kstr] = joint->PositionLimitsInternal->data[b_kstr];

      }



      b->data[static_cast<int32_T>(m + 1.0) - 1] = -e->data[0];

      m = tmp;

    }



    n += pnum;

  }



  cartesian_waypoi_emxFree_real_T(&s);

  b_kstr = A->size[0] * A->size[1];

  c = obj->Solver->ConstraintMatrix->size[0] * obj->Solver->

    ConstraintMatrix->size[1];

  obj->Solver->ConstraintMatrix->size[0] = A->size[0];

  obj->Solver->ConstraintMatrix->size[1] = A->size[1];

  cartes_emxEnsureCapacity_real_T(obj->Solver->ConstraintMatrix, c);

  loop_ub = b_kstr - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    obj->Solver->ConstraintMatrix->data[b_kstr] = A->data[b_kstr];

  }



  cartesian_waypoi_emxFree_real_T(&A);

  b_kstr = obj->Solver->ConstraintBound->size[0];

  obj->Solver->ConstraintBound->size[0] = b->size[0];

  cartes_emxEnsureCapacity_real_T(obj->Solver->ConstraintBound, b_kstr);

  loop_ub = b->size[0];

  for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {

    obj->Solver->ConstraintBound->data[b_kstr] = b->data[b_kstr];

  }



  obj_0 = obj->RigidBodyTreeInternal;

  b_kstr = e->size[0] * e->size[1];

  e->size[0] = static_cast<int32_T>(obj_0->PositionNumber);

  e->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(e, b_kstr);

  loop_ub = (static_cast<int32_T>(obj_0->PositionNumber) << 1) - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    e->data[b_kstr] = 0.0;

  }



  n = 1.0;

  m = obj_0->NumBodies;

  c = static_cast<int32_T>(m) - 1;

  if (0 <= c) {

    for (b_kstr = 0; b_kstr < 5; b_kstr++) {

      b_0[b_kstr] = tmp_0[b_kstr];

    }

  }



  for (b_i = 0; b_i <= c; b_i++) {

    body = obj_0->Bodies[b_i];

    b_kstr = a->size[0] * a->size[1];

    a->size[0] = 1;

    a->size[1] = body->JointInternal->Type->size[1];

    cartes_emxEnsureCapacity_char_T(a, b_kstr);

    loop_ub = body->JointInternal->Type->size[0] * body->JointInternal->

      Type->size[1] - 1;

    for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

      a->data[b_kstr] = body->JointInternal->Type->data[b_kstr];

    }



    b_bool = false;

    if (a->size[1] == 5) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 5) {

          loop_ub = b_kstr - 1;

          if (a->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (!b_bool) {

      pnum = body->JointInternal->PositionNumber;

      tmp = n + pnum;

      if (n > tmp - 1.0) {

        d = 0;

      } else {

        d = static_cast<int32_T>(n) - 1;

      }



      joint = body->JointInternal;

      loop_ub = joint->PositionLimitsInternal->size[0];

      for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {

        e->data[d + b_kstr] = joint->PositionLimitsInternal->data[b_kstr];

      }



      loop_ub = joint->PositionLimitsInternal->size[0];

      for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {

        e->data[(d + b_kstr) + e->size[0]] = joint->PositionLimitsInternal->

          data[b_kstr + joint->PositionLimitsInternal->size[0]];

      }



      n = tmp;

    }

  }



  cartesian_waypoi_emxFree_char_T(&a);

  b_kstr = obj->Limits->size[0] * obj->Limits->size[1];

  obj->Limits->size[0] = e->size[0];

  obj->Limits->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(obj->Limits, b_kstr);

  loop_ub = e->size[0] * e->size[1] - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    obj->Limits->data[b_kstr] = e->data[b_kstr];

  }



  cartesian_waypoi_emxFree_real_T(&e);

  obj->Solver->ExtraArgs = iobj_0;

  for (b_kstr = 0; b_kstr < 36; b_kstr++) {

    obj->Solver->ExtraArgs->WeightMatrix[b_kstr] = 0.0;

  }



  obj->Solver->ExtraArgs->Robot = obj->RigidBodyTreeInternal;

  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    b_I[b_kstr] = 0;

  }



  b_I[0] = 1;

  b_I[5] = 1;

  b_I[10] = 1;

  b_I[15] = 1;

  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->Solver->ExtraArgs->Tform[b_kstr] = b_I[b_kstr];

  }



  obj->Solver->ExtraArgs->BodyName->size[0] = 1;

  obj->Solver->ExtraArgs->BodyName->size[1] = 0;

  b_kstr = obj->Solver->ExtraArgs->ErrTemp->size[0];

  obj->Solver->ExtraArgs->ErrTemp->size[0] = 6;

  cartes_emxEnsureCapacity_real_T(obj->Solver->ExtraArgs->ErrTemp, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    obj->Solver->ExtraArgs->ErrTemp->data[b_kstr] = 0.0;

  }



  obj->Solver->ExtraArgs->CostTemp = 0.0;

  b_kstr = b->size[0];

  b->size[0] = static_cast<int32_T>(obj->RigidBodyTreeInternal->PositionNumber);

  cartes_emxEnsureCapacity_real_T(b, b_kstr);

  loop_ub = static_cast<int32_T>(obj->RigidBodyTreeInternal->PositionNumber);

  for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {

    b->data[b_kstr] = 0.0;

  }



  b_kstr = obj->Solver->ExtraArgs->GradTemp->size[0];

  obj->Solver->ExtraArgs->GradTemp->size[0] = b->size[0];

  cartes_emxEnsureCapacity_real_T(obj->Solver->ExtraArgs->GradTemp, b_kstr);

  loop_ub = b->size[0];

  for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {

    obj->Solver->ExtraArgs->GradTemp->data[b_kstr] = b->data[b_kstr];

  }



  cartesian_waypoi_emxFree_real_T(&b);

}



static void c_inverseKinematics_setPoseGoal(b_inverseKinematics_cartesian_T *obj,

  const real_T tform[16], const real_T weights[6])

{

  real_T weightMatrix[36];

  f_robotics_manip_internal_IKE_T *args;

  int32_T b_j;

  static const char_T tmp[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',

    'e', 'e' };



  memset(&weightMatrix[0], 0, 36U * sizeof(real_T));

  for (b_j = 0; b_j < 6; b_j++) {

    weightMatrix[b_j + 6 * b_j] = weights[b_j];

  }



  args = obj->Solver->ExtraArgs;

  for (b_j = 0; b_j < 36; b_j++) {

    args->WeightMatrix[b_j] = weightMatrix[b_j];

  }



  b_j = args->BodyName->size[0] * args->BodyName->size[1];

  args->BodyName->size[0] = 1;

  args->BodyName->size[1] = 11;

  cartes_emxEnsureCapacity_char_T(args->BodyName, b_j);

  for (b_j = 0; b_j < 11; b_j++) {

    args->BodyName->data[b_j] = tmp[b_j];

  }



  for (b_j = 0; b_j < 16; b_j++) {

    args->Tform[b_j] = tform[b_j];

  }

}



static void RigidBodyTree_validateConfigu_e(x_robotics_manip_internal_Rig_T *obj,

  real_T Q[6])

{

  emxArray_real_T_cartesian_way_T *limits;

  boolean_T ubOK[6];

  boolean_T lbOK[6];

  real_T k;

  w_robotics_manip_internal_Rig_T *body;

  real_T pnum;

  int32_T c;

  int32_T f;

  int32_T ii_data[6];

  boolean_T b_bool;

  emxArray_char_T_cartesian_way_T *a;

  int32_T b_kstr;

  char_T b[5];

  c_rigidBodyJoint_cartesian_wa_T *obj_0;

  int32_T idx;

  int32_T loop_ub;

  emxArray_real_T_cartesian_way_T *limits_0;

  emxArray_real_T_cartesian_way_T *limits_1;

  static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };



  boolean_T guard1 = false;

  int32_T exitg1;

  boolean_T exitg2;

  cartesian_waypoi_emxInit_real_T(&limits, 2);

  b_kstr = limits->size[0] * limits->size[1];

  limits->size[0] = static_cast<int32_T>(obj->PositionNumber);

  limits->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(limits, b_kstr);

  loop_ub = (static_cast<int32_T>(obj->PositionNumber) << 1) - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    limits->data[b_kstr] = 0.0;

  }



  k = 1.0;

  pnum = obj->NumBodies;

  c = static_cast<int32_T>(pnum) - 1;

  cartesian_waypoi_emxInit_char_T(&a, 2);

  if (0 <= c) {

    for (b_kstr = 0; b_kstr < 5; b_kstr++) {

      b[b_kstr] = tmp[b_kstr];

    }

  }



  for (idx = 0; idx <= c; idx++) {

    body = obj->Bodies[idx];

    b_kstr = a->size[0] * a->size[1];

    a->size[0] = 1;

    a->size[1] = body->JointInternal->Type->size[1];

    cartes_emxEnsureCapacity_char_T(a, b_kstr);

    loop_ub = body->JointInternal->Type->size[0] * body->JointInternal->

      Type->size[1] - 1;

    for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

      a->data[b_kstr] = body->JointInternal->Type->data[b_kstr];

    }



    b_bool = false;

    if (a->size[1] == 5) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 5) {

          loop_ub = b_kstr - 1;

          if (a->data[loop_ub] != b[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (!b_bool) {

      pnum = body->JointInternal->PositionNumber;

      pnum += k;

      if (k > pnum - 1.0) {

        f = 0;

      } else {

        f = static_cast<int32_T>(k) - 1;

      }



      obj_0 = body->JointInternal;

      loop_ub = obj_0->PositionLimitsInternal->size[0];

      for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {

        limits->data[f + b_kstr] = obj_0->PositionLimitsInternal->data[b_kstr];

      }



      loop_ub = obj_0->PositionLimitsInternal->size[0];

      for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {

        limits->data[(f + b_kstr) + limits->size[0]] =

          obj_0->PositionLimitsInternal->data[b_kstr +

          obj_0->PositionLimitsInternal->size[0]];

      }



      k = pnum;

    }

  }



  cartesian_waypoi_emxFree_char_T(&a);

  cartesian_waypoi_emxInit_real_T(&limits_0, 1);

  loop_ub = limits->size[0];

  b_kstr = limits_0->size[0];

  limits_0->size[0] = loop_ub;

  cartes_emxEnsureCapacity_real_T(limits_0, b_kstr);

  for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {

    limits_0->data[b_kstr] = limits->data[b_kstr + limits->size[0]] +

      4.4408920985006262E-16;

  }



  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    ubOK[b_kstr] = (Q[b_kstr] <= limits_0->data[b_kstr]);

  }



  cartesian_waypoi_emxFree_real_T(&limits_0);

  cartesian_waypoi_emxInit_real_T(&limits_1, 1);

  loop_ub = limits->size[0];

  b_kstr = limits_1->size[0];

  limits_1->size[0] = loop_ub;

  cartes_emxEnsureCapacity_real_T(limits_1, b_kstr);

  for (b_kstr = 0; b_kstr < loop_ub; b_kstr++) {

    limits_1->data[b_kstr] = limits->data[b_kstr] - 4.4408920985006262E-16;

  }



  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    lbOK[b_kstr] = (Q[b_kstr] >= limits_1->data[b_kstr]);

  }



  cartesian_waypoi_emxFree_real_T(&limits_1);

  b_bool = true;

  idx = 0;

  exitg2 = false;

  while ((!exitg2) && (idx < 6)) {

    if (!ubOK[idx]) {

      b_bool = false;

      exitg2 = true;

    } else {

      idx++;

    }

  }



  guard1 = false;

  if (b_bool) {

    b_bool = true;

    idx = 0;

    exitg2 = false;

    while ((!exitg2) && (idx < 6)) {

      if (!lbOK[idx]) {

        b_bool = false;

        exitg2 = true;

      } else {

        idx++;

      }

    }



    if (b_bool) {

    } else {

      guard1 = true;

    }

  } else {

    guard1 = true;

  }



  if (guard1) {

    idx = 0;

    b_kstr = 1;

    exitg2 = false;

    while ((!exitg2) && (b_kstr - 1 < 6)) {

      if (!ubOK[b_kstr - 1]) {

        idx++;

        ii_data[idx - 1] = b_kstr;

        if (idx >= 6) {

          exitg2 = true;

        } else {

          b_kstr++;

        }

      } else {

        b_kstr++;

      }

    }



    if (1 > idx) {

      idx = 0;

    }



    for (b_kstr = 0; b_kstr < idx; b_kstr++) {

      Q[ii_data[b_kstr] - 1] = limits->data[(ii_data[b_kstr] + limits->size[0])

        - 1];

    }



    idx = 0;

    b_kstr = 1;

    exitg2 = false;

    while ((!exitg2) && (b_kstr - 1 < 6)) {

      if (!lbOK[b_kstr - 1]) {

        idx++;

        ii_data[idx - 1] = b_kstr;

        if (idx >= 6) {

          exitg2 = true;

        } else {

          b_kstr++;

        }

      } else {

        b_kstr++;

      }

    }



    if (1 > idx) {

      idx = 0;

    }



    for (b_kstr = 0; b_kstr < idx; b_kstr++) {

      Q[ii_data[b_kstr] - 1] = limits->data[ii_data[b_kstr] - 1];

    }

  }



  cartesian_waypoi_emxFree_real_T(&limits);

}



static boolean_T cartesian_waypoints_plan_strcmp(const

  emxArray_char_T_cartesian_way_T *a, const emxArray_char_T_cartesian_way_T *b)

{

  boolean_T b_bool;

  int32_T kstr;

  int32_T b_kstr;

  boolean_T d;

  int32_T exitg1;

  b_bool = false;

  d = (a->size[1] == 0);

  if (d && (b->size[1] == 0)) {

    b_bool = true;

  } else if (a->size[1] != b->size[1]) {

  } else {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 <= b->size[1] - 1) {

        kstr = b_kstr - 1;

        if (a->data[kstr] != b->data[kstr]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  return b_bool;

}



static void ca_rigidBodyJoint_get_JointAxis(const

  c_rigidBodyJoint_cartesian_wa_T *obj, real_T ax[3])

{

  emxArray_char_T_cartesian_way_T *a;

  static const char_T tmp[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_0[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  boolean_T guard1 = false;

  int32_T exitg1;

  cartesian_waypoi_emxInit_char_T(&a, 2);

  cartesian_waypoints_planner_B.b_kstr_n = a->size[0] * a->size[1];

  a->size[0] = 1;

  a->size[1] = obj->Type->size[1];

  cartes_emxEnsureCapacity_char_T(a, cartesian_waypoints_planner_B.b_kstr_n);

  cartesian_waypoints_planner_B.loop_ub_m = obj->Type->size[0] * obj->Type->

    size[1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr_n = 0;

       cartesian_waypoints_planner_B.b_kstr_n <=

       cartesian_waypoints_planner_B.loop_ub_m;

       cartesian_waypoints_planner_B.b_kstr_n++) {

    a->data[cartesian_waypoints_planner_B.b_kstr_n] = obj->Type->

      data[cartesian_waypoints_planner_B.b_kstr_n];

  }



  for (cartesian_waypoints_planner_B.b_kstr_n = 0;

       cartesian_waypoints_planner_B.b_kstr_n < 8;

       cartesian_waypoints_planner_B.b_kstr_n++) {

    cartesian_waypoints_planner_B.b_a[cartesian_waypoints_planner_B.b_kstr_n] =

      tmp[cartesian_waypoints_planner_B.b_kstr_n];

  }



  cartesian_waypoints_planner_B.b_bool_p = false;

  if (a->size[1] == 8) {

    cartesian_waypoints_planner_B.b_kstr_n = 1;

    do {

      exitg1 = 0;

      if (cartesian_waypoints_planner_B.b_kstr_n - 1 < 8) {

        cartesian_waypoints_planner_B.loop_ub_m =

          cartesian_waypoints_planner_B.b_kstr_n - 1;

        if (a->data[cartesian_waypoints_planner_B.loop_ub_m] !=

            cartesian_waypoints_planner_B.b_a[cartesian_waypoints_planner_B.loop_ub_m])

        {

          exitg1 = 1;

        } else {

          cartesian_waypoints_planner_B.b_kstr_n++;

        }

      } else {

        cartesian_waypoints_planner_B.b_bool_p = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  guard1 = false;

  if (cartesian_waypoints_planner_B.b_bool_p) {

    guard1 = true;

  } else {

    cartesian_waypoints_planner_B.b_kstr_n = a->size[0] * a->size[1];

    a->size[0] = 1;

    a->size[1] = obj->Type->size[1];

    cartes_emxEnsureCapacity_char_T(a, cartesian_waypoints_planner_B.b_kstr_n);

    cartesian_waypoints_planner_B.loop_ub_m = obj->Type->size[0] * obj->

      Type->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_kstr_n = 0;

         cartesian_waypoints_planner_B.b_kstr_n <=

         cartesian_waypoints_planner_B.loop_ub_m;

         cartesian_waypoints_planner_B.b_kstr_n++) {

      a->data[cartesian_waypoints_planner_B.b_kstr_n] = obj->Type->

        data[cartesian_waypoints_planner_B.b_kstr_n];

    }



    for (cartesian_waypoints_planner_B.b_kstr_n = 0;

         cartesian_waypoints_planner_B.b_kstr_n < 9;

         cartesian_waypoints_planner_B.b_kstr_n++) {

      cartesian_waypoints_planner_B.b_m[cartesian_waypoints_planner_B.b_kstr_n] =

        tmp_0[cartesian_waypoints_planner_B.b_kstr_n];

    }



    cartesian_waypoints_planner_B.b_bool_p = false;

    if (a->size[1] == 9) {

      cartesian_waypoints_planner_B.b_kstr_n = 1;

      do {

        exitg1 = 0;

        if (cartesian_waypoints_planner_B.b_kstr_n - 1 < 9) {

          cartesian_waypoints_planner_B.loop_ub_m =

            cartesian_waypoints_planner_B.b_kstr_n - 1;

          if (a->data[cartesian_waypoints_planner_B.loop_ub_m] !=

              cartesian_waypoints_planner_B.b_m[cartesian_waypoints_planner_B.loop_ub_m])

          {

            exitg1 = 1;

          } else {

            cartesian_waypoints_planner_B.b_kstr_n++;

          }

        } else {

          cartesian_waypoints_planner_B.b_bool_p = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (cartesian_waypoints_planner_B.b_bool_p) {

      guard1 = true;

    } else {

      ax[0] = (rtNaN);

      ax[1] = (rtNaN);

      ax[2] = (rtNaN);

    }

  }



  if (guard1) {

    ax[0] = obj->JointAxisInternal[0];

    ax[1] = obj->JointAxisInternal[1];

    ax[2] = obj->JointAxisInternal[2];

  }



  cartesian_waypoi_emxFree_char_T(&a);

}



static void cartesian_waypoints_planner_cat(real_T varargin_1, real_T varargin_2,

  real_T varargin_3, real_T varargin_4, real_T varargin_5, real_T varargin_6,

  real_T varargin_7, real_T varargin_8, real_T varargin_9, real_T y[9])

{

  y[0] = varargin_1;

  y[1] = varargin_2;

  y[2] = varargin_3;

  y[3] = varargin_4;

  y[4] = varargin_5;

  y[5] = varargin_6;

  y[6] = varargin_7;

  y[7] = varargin_8;

  y[8] = varargin_9;

}



static void rigidBodyJoint_transformBodyT_e(const

  c_rigidBodyJoint_cartesian_wa_T *obj, const real_T q_data[], const int32_T

  *q_size, real_T T[16])

{

  emxArray_char_T_cartesian_way_T *switch_expression;

  static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };



  static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  int32_T exitg1;

  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  cartesian_waypoints_planner_B.b_kstr_b = switch_expression->size[0] *

    switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = obj->Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression,

    cartesian_waypoints_planner_B.b_kstr_b);

  cartesian_waypoints_planner_B.loop_ub_o = obj->Type->size[0] * obj->Type->

    size[1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr_b = 0;

       cartesian_waypoints_planner_B.b_kstr_b <=

       cartesian_waypoints_planner_B.loop_ub_o;

       cartesian_waypoints_planner_B.b_kstr_b++) {

    switch_expression->data[cartesian_waypoints_planner_B.b_kstr_b] = obj->

      Type->data[cartesian_waypoints_planner_B.b_kstr_b];

  }



  for (cartesian_waypoints_planner_B.b_kstr_b = 0;

       cartesian_waypoints_planner_B.b_kstr_b < 5;

       cartesian_waypoints_planner_B.b_kstr_b++) {

    cartesian_waypoints_planner_B.b_js[cartesian_waypoints_planner_B.b_kstr_b] =

      tmp[cartesian_waypoints_planner_B.b_kstr_b];

  }



  cartesian_waypoints_planner_B.b_bool_b = false;

  if (switch_expression->size[1] == 5) {

    cartesian_waypoints_planner_B.b_kstr_b = 1;

    do {

      exitg1 = 0;

      if (cartesian_waypoints_planner_B.b_kstr_b - 1 < 5) {

        cartesian_waypoints_planner_B.loop_ub_o =

          cartesian_waypoints_planner_B.b_kstr_b - 1;

        if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_o] !=

            cartesian_waypoints_planner_B.b_js[cartesian_waypoints_planner_B.loop_ub_o])

        {

          exitg1 = 1;

        } else {

          cartesian_waypoints_planner_B.b_kstr_b++;

        }

      } else {

        cartesian_waypoints_planner_B.b_bool_b = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (cartesian_waypoints_planner_B.b_bool_b) {

    cartesian_waypoints_planner_B.b_kstr_b = 0;

  } else {

    for (cartesian_waypoints_planner_B.b_kstr_b = 0;

         cartesian_waypoints_planner_B.b_kstr_b < 8;

         cartesian_waypoints_planner_B.b_kstr_b++) {

      cartesian_waypoints_planner_B.b_f[cartesian_waypoints_planner_B.b_kstr_b] =

        tmp_0[cartesian_waypoints_planner_B.b_kstr_b];

    }



    cartesian_waypoints_planner_B.b_bool_b = false;

    if (switch_expression->size[1] == 8) {

      cartesian_waypoints_planner_B.b_kstr_b = 1;

      do {

        exitg1 = 0;

        if (cartesian_waypoints_planner_B.b_kstr_b - 1 < 8) {

          cartesian_waypoints_planner_B.loop_ub_o =

            cartesian_waypoints_planner_B.b_kstr_b - 1;

          if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_o]

              !=

              cartesian_waypoints_planner_B.b_f[cartesian_waypoints_planner_B.loop_ub_o])

          {

            exitg1 = 1;

          } else {

            cartesian_waypoints_planner_B.b_kstr_b++;

          }

        } else {

          cartesian_waypoints_planner_B.b_bool_b = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (cartesian_waypoints_planner_B.b_bool_b) {

      cartesian_waypoints_planner_B.b_kstr_b = 1;

    } else {

      cartesian_waypoints_planner_B.b_kstr_b = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (cartesian_waypoints_planner_B.b_kstr_b) {

   case 0:

    memset(&cartesian_waypoints_planner_B.TJ[0], 0, sizeof(real_T) << 4U);

    cartesian_waypoints_planner_B.TJ[0] = 1.0;

    cartesian_waypoints_planner_B.TJ[5] = 1.0;

    cartesian_waypoints_planner_B.TJ[10] = 1.0;

    cartesian_waypoints_planner_B.TJ[15] = 1.0;

    break;



   case 1:

    ca_rigidBodyJoint_get_JointAxis(obj, cartesian_waypoints_planner_B.v_c);

    cartesian_waypoints_planner_B.loop_ub_o = (*q_size != 0) - 1;

    cartesian_waypoints_planner_B.result_data[0] =

      cartesian_waypoints_planner_B.v_c[0];

    cartesian_waypoints_planner_B.result_data[1] =

      cartesian_waypoints_planner_B.v_c[1];

    cartesian_waypoints_planner_B.result_data[2] =

      cartesian_waypoints_planner_B.v_c[2];

    for (cartesian_waypoints_planner_B.b_kstr_b = 0;

         cartesian_waypoints_planner_B.b_kstr_b <=

         cartesian_waypoints_planner_B.loop_ub_o;

         cartesian_waypoints_planner_B.b_kstr_b++) {

      cartesian_waypoints_planner_B.result_data[3] = q_data[0];

    }



    cartesian_waypoints_planner_B.cth = 1.0 / sqrt

      ((cartesian_waypoints_planner_B.result_data[0] *

        cartesian_waypoints_planner_B.result_data[0] +

        cartesian_waypoints_planner_B.result_data[1] *

        cartesian_waypoints_planner_B.result_data[1]) +

       cartesian_waypoints_planner_B.result_data[2] *

       cartesian_waypoints_planner_B.result_data[2]);

    cartesian_waypoints_planner_B.v_c[0] =

      cartesian_waypoints_planner_B.result_data[0] *

      cartesian_waypoints_planner_B.cth;

    cartesian_waypoints_planner_B.v_c[1] =

      cartesian_waypoints_planner_B.result_data[1] *

      cartesian_waypoints_planner_B.cth;

    cartesian_waypoints_planner_B.v_c[2] =

      cartesian_waypoints_planner_B.result_data[2] *

      cartesian_waypoints_planner_B.cth;

    cartesian_waypoints_planner_B.cth = cos

      (cartesian_waypoints_planner_B.result_data[3]);

    cartesian_waypoints_planner_B.sth = sin

      (cartesian_waypoints_planner_B.result_data[3]);

    cartesian_waypoints_planner_B.tempR_tmp = cartesian_waypoints_planner_B.v_c

      [1] * cartesian_waypoints_planner_B.v_c[0] * (1.0 -

      cartesian_waypoints_planner_B.cth);

    cartesian_waypoints_planner_B.tempR_tmp_f =

      cartesian_waypoints_planner_B.v_c[2] * cartesian_waypoints_planner_B.sth;

    cartesian_waypoints_planner_B.tempR_tmp_d =

      cartesian_waypoints_planner_B.v_c[2] * cartesian_waypoints_planner_B.v_c[0]

      * (1.0 - cartesian_waypoints_planner_B.cth);

    cartesian_waypoints_planner_B.tempR_tmp_j =

      cartesian_waypoints_planner_B.v_c[1] * cartesian_waypoints_planner_B.sth;

    cartesian_waypoints_planner_B.tempR_tmp_i =

      cartesian_waypoints_planner_B.v_c[2] * cartesian_waypoints_planner_B.v_c[1]

      * (1.0 - cartesian_waypoints_planner_B.cth);

    cartesian_waypoints_planner_B.sth *= cartesian_waypoints_planner_B.v_c[0];

    cartesian_waypoints_planner_cat(cartesian_waypoints_planner_B.v_c[0] *

      cartesian_waypoints_planner_B.v_c[0] * (1.0 -

      cartesian_waypoints_planner_B.cth) + cartesian_waypoints_planner_B.cth,

      cartesian_waypoints_planner_B.tempR_tmp -

      cartesian_waypoints_planner_B.tempR_tmp_f,

      cartesian_waypoints_planner_B.tempR_tmp_d +

      cartesian_waypoints_planner_B.tempR_tmp_j,

      cartesian_waypoints_planner_B.tempR_tmp +

      cartesian_waypoints_planner_B.tempR_tmp_f,

      cartesian_waypoints_planner_B.v_c[1] * cartesian_waypoints_planner_B.v_c[1]

      * (1.0 - cartesian_waypoints_planner_B.cth) +

      cartesian_waypoints_planner_B.cth,

      cartesian_waypoints_planner_B.tempR_tmp_i -

      cartesian_waypoints_planner_B.sth,

      cartesian_waypoints_planner_B.tempR_tmp_d -

      cartesian_waypoints_planner_B.tempR_tmp_j,

      cartesian_waypoints_planner_B.tempR_tmp_i +

      cartesian_waypoints_planner_B.sth, cartesian_waypoints_planner_B.v_c[2] *

      cartesian_waypoints_planner_B.v_c[2] * (1.0 -

      cartesian_waypoints_planner_B.cth) + cartesian_waypoints_planner_B.cth,

      cartesian_waypoints_planner_B.tempR_o);

    for (cartesian_waypoints_planner_B.b_kstr_b = 0;

         cartesian_waypoints_planner_B.b_kstr_b < 3;

         cartesian_waypoints_planner_B.b_kstr_b++) {

      cartesian_waypoints_planner_B.loop_ub_o =

        cartesian_waypoints_planner_B.b_kstr_b + 1;

      cartesian_waypoints_planner_B.R_o[cartesian_waypoints_planner_B.loop_ub_o

        - 1] = cartesian_waypoints_planner_B.tempR_o

        [(cartesian_waypoints_planner_B.loop_ub_o - 1) * 3];

      cartesian_waypoints_planner_B.loop_ub_o =

        cartesian_waypoints_planner_B.b_kstr_b + 1;

      cartesian_waypoints_planner_B.R_o[cartesian_waypoints_planner_B.loop_ub_o

        + 2] = cartesian_waypoints_planner_B.tempR_o

        [(cartesian_waypoints_planner_B.loop_ub_o - 1) * 3 + 1];

      cartesian_waypoints_planner_B.loop_ub_o =

        cartesian_waypoints_planner_B.b_kstr_b + 1;

      cartesian_waypoints_planner_B.R_o[cartesian_waypoints_planner_B.loop_ub_o

        + 5] = cartesian_waypoints_planner_B.tempR_o

        [(cartesian_waypoints_planner_B.loop_ub_o - 1) * 3 + 2];

    }



    memset(&cartesian_waypoints_planner_B.TJ[0], 0, sizeof(real_T) << 4U);

    for (cartesian_waypoints_planner_B.b_kstr_b = 0;

         cartesian_waypoints_planner_B.b_kstr_b < 3;

         cartesian_waypoints_planner_B.b_kstr_b++) {

      cartesian_waypoints_planner_B.loop_ub_o =

        cartesian_waypoints_planner_B.b_kstr_b << 2;

      cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.loop_ub_o] =

        cartesian_waypoints_planner_B.R_o[3 *

        cartesian_waypoints_planner_B.b_kstr_b];

      cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.loop_ub_o +

        1] = cartesian_waypoints_planner_B.R_o[3 *

        cartesian_waypoints_planner_B.b_kstr_b + 1];

      cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.loop_ub_o +

        2] = cartesian_waypoints_planner_B.R_o[3 *

        cartesian_waypoints_planner_B.b_kstr_b + 2];

    }



    cartesian_waypoints_planner_B.TJ[15] = 1.0;

    break;



   default:

    ca_rigidBodyJoint_get_JointAxis(obj, cartesian_waypoints_planner_B.v_c);

    memset(&cartesian_waypoints_planner_B.tempR_o[0], 0, 9U * sizeof(real_T));

    cartesian_waypoints_planner_B.tempR_o[0] = 1.0;

    cartesian_waypoints_planner_B.tempR_o[4] = 1.0;

    cartesian_waypoints_planner_B.tempR_o[8] = 1.0;

    for (cartesian_waypoints_planner_B.b_kstr_b = 0;

         cartesian_waypoints_planner_B.b_kstr_b < 3;

         cartesian_waypoints_planner_B.b_kstr_b++) {

      cartesian_waypoints_planner_B.loop_ub_o =

        cartesian_waypoints_planner_B.b_kstr_b << 2;

      cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.loop_ub_o] =

        cartesian_waypoints_planner_B.tempR_o[3 *

        cartesian_waypoints_planner_B.b_kstr_b];

      cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.loop_ub_o +

        1] = cartesian_waypoints_planner_B.tempR_o[3 *

        cartesian_waypoints_planner_B.b_kstr_b + 1];

      cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.loop_ub_o +

        2] = cartesian_waypoints_planner_B.tempR_o[3 *

        cartesian_waypoints_planner_B.b_kstr_b + 2];

      cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.b_kstr_b +

        12] =

        cartesian_waypoints_planner_B.v_c[cartesian_waypoints_planner_B.b_kstr_b]

        * q_data[0];

    }



    cartesian_waypoints_planner_B.TJ[3] = 0.0;

    cartesian_waypoints_planner_B.TJ[7] = 0.0;

    cartesian_waypoints_planner_B.TJ[11] = 0.0;

    cartesian_waypoints_planner_B.TJ[15] = 1.0;

    break;

  }



  for (cartesian_waypoints_planner_B.b_kstr_b = 0;

       cartesian_waypoints_planner_B.b_kstr_b < 16;

       cartesian_waypoints_planner_B.b_kstr_b++) {

    cartesian_waypoints_planner_B.a[cartesian_waypoints_planner_B.b_kstr_b] =

      obj->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_b];

  }



  for (cartesian_waypoints_planner_B.b_kstr_b = 0;

       cartesian_waypoints_planner_B.b_kstr_b < 16;

       cartesian_waypoints_planner_B.b_kstr_b++) {

    cartesian_waypoints_planner_B.b[cartesian_waypoints_planner_B.b_kstr_b] =

      obj->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_b];

  }



  for (cartesian_waypoints_planner_B.b_kstr_b = 0;

       cartesian_waypoints_planner_B.b_kstr_b < 4;

       cartesian_waypoints_planner_B.b_kstr_b++) {

    for (cartesian_waypoints_planner_B.loop_ub_o = 0;

         cartesian_waypoints_planner_B.loop_ub_o < 4;

         cartesian_waypoints_planner_B.loop_ub_o++) {

      cartesian_waypoints_planner_B.a_tmp_tmp =

        cartesian_waypoints_planner_B.loop_ub_o << 2;

      cartesian_waypoints_planner_B.a_tmp =

        cartesian_waypoints_planner_B.b_kstr_b +

        cartesian_waypoints_planner_B.a_tmp_tmp;

      cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.a_tmp] =

        0.0;

      cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.a_tmp] +=

        cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.a_tmp_tmp]

        * cartesian_waypoints_planner_B.a[cartesian_waypoints_planner_B.b_kstr_b];

      cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.a_tmp] +=

        cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.a_tmp_tmp

        + 1] *

        cartesian_waypoints_planner_B.a[cartesian_waypoints_planner_B.b_kstr_b +

        4];

      cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.a_tmp] +=

        cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.a_tmp_tmp

        + 2] *

        cartesian_waypoints_planner_B.a[cartesian_waypoints_planner_B.b_kstr_b +

        8];

      cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.a_tmp] +=

        cartesian_waypoints_planner_B.TJ[cartesian_waypoints_planner_B.a_tmp_tmp

        + 3] *

        cartesian_waypoints_planner_B.a[cartesian_waypoints_planner_B.b_kstr_b +

        12];

    }



    for (cartesian_waypoints_planner_B.loop_ub_o = 0;

         cartesian_waypoints_planner_B.loop_ub_o < 4;

         cartesian_waypoints_planner_B.loop_ub_o++) {

      cartesian_waypoints_planner_B.a_tmp_tmp =

        cartesian_waypoints_planner_B.loop_ub_o << 2;

      cartesian_waypoints_planner_B.a_tmp =

        cartesian_waypoints_planner_B.b_kstr_b +

        cartesian_waypoints_planner_B.a_tmp_tmp;

      T[cartesian_waypoints_planner_B.a_tmp] = 0.0;

      T[cartesian_waypoints_planner_B.a_tmp] +=

        cartesian_waypoints_planner_B.b[cartesian_waypoints_planner_B.a_tmp_tmp]

        * cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.b_kstr_b];

      T[cartesian_waypoints_planner_B.a_tmp] +=

        cartesian_waypoints_planner_B.b[cartesian_waypoints_planner_B.a_tmp_tmp

        + 1] *

        cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.b_kstr_b

        + 4];

      T[cartesian_waypoints_planner_B.a_tmp] +=

        cartesian_waypoints_planner_B.b[cartesian_waypoints_planner_B.a_tmp_tmp

        + 2] *

        cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.b_kstr_b

        + 8];

      T[cartesian_waypoints_planner_B.a_tmp] +=

        cartesian_waypoints_planner_B.b[cartesian_waypoints_planner_B.a_tmp_tmp

        + 3] *

        cartesian_waypoints_planner_B.a_p[cartesian_waypoints_planner_B.b_kstr_b

        + 12];

    }

  }

}



static void rigidBodyJoint_transformBodyToP(const

  c_rigidBodyJoint_cartesian_wa_T *obj, real_T T[16])

{

  emxArray_char_T_cartesian_way_T *switch_expression;

  static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };



  static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  int32_T exitg1;

  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  cartesian_waypoints_planner_B.b_kstr_g = switch_expression->size[0] *

    switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = obj->Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression,

    cartesian_waypoints_planner_B.b_kstr_g);

  cartesian_waypoints_planner_B.loop_ub_d = obj->Type->size[0] * obj->Type->

    size[1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr_g = 0;

       cartesian_waypoints_planner_B.b_kstr_g <=

       cartesian_waypoints_planner_B.loop_ub_d;

       cartesian_waypoints_planner_B.b_kstr_g++) {

    switch_expression->data[cartesian_waypoints_planner_B.b_kstr_g] = obj->

      Type->data[cartesian_waypoints_planner_B.b_kstr_g];

  }



  for (cartesian_waypoints_planner_B.b_kstr_g = 0;

       cartesian_waypoints_planner_B.b_kstr_g < 5;

       cartesian_waypoints_planner_B.b_kstr_g++) {

    cartesian_waypoints_planner_B.b_h[cartesian_waypoints_planner_B.b_kstr_g] =

      tmp[cartesian_waypoints_planner_B.b_kstr_g];

  }



  cartesian_waypoints_planner_B.b_bool_c = false;

  if (switch_expression->size[1] == 5) {

    cartesian_waypoints_planner_B.b_kstr_g = 1;

    do {

      exitg1 = 0;

      if (cartesian_waypoints_planner_B.b_kstr_g - 1 < 5) {

        cartesian_waypoints_planner_B.loop_ub_d =

          cartesian_waypoints_planner_B.b_kstr_g - 1;

        if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_d] !=

            cartesian_waypoints_planner_B.b_h[cartesian_waypoints_planner_B.loop_ub_d])

        {

          exitg1 = 1;

        } else {

          cartesian_waypoints_planner_B.b_kstr_g++;

        }

      } else {

        cartesian_waypoints_planner_B.b_bool_c = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (cartesian_waypoints_planner_B.b_bool_c) {

    cartesian_waypoints_planner_B.b_kstr_g = 0;

  } else {

    for (cartesian_waypoints_planner_B.b_kstr_g = 0;

         cartesian_waypoints_planner_B.b_kstr_g < 8;

         cartesian_waypoints_planner_B.b_kstr_g++) {

      cartesian_waypoints_planner_B.b_g[cartesian_waypoints_planner_B.b_kstr_g] =

        tmp_0[cartesian_waypoints_planner_B.b_kstr_g];

    }



    cartesian_waypoints_planner_B.b_bool_c = false;

    if (switch_expression->size[1] == 8) {

      cartesian_waypoints_planner_B.b_kstr_g = 1;

      do {

        exitg1 = 0;

        if (cartesian_waypoints_planner_B.b_kstr_g - 1 < 8) {

          cartesian_waypoints_planner_B.loop_ub_d =

            cartesian_waypoints_planner_B.b_kstr_g - 1;

          if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_d]

              !=

              cartesian_waypoints_planner_B.b_g[cartesian_waypoints_planner_B.loop_ub_d])

          {

            exitg1 = 1;

          } else {

            cartesian_waypoints_planner_B.b_kstr_g++;

          }

        } else {

          cartesian_waypoints_planner_B.b_bool_c = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (cartesian_waypoints_planner_B.b_bool_c) {

      cartesian_waypoints_planner_B.b_kstr_g = 1;

    } else {

      cartesian_waypoints_planner_B.b_kstr_g = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (cartesian_waypoints_planner_B.b_kstr_g) {

   case 0:

    memset(&cartesian_waypoints_planner_B.TJ_l[0], 0, sizeof(real_T) << 4U);

    cartesian_waypoints_planner_B.TJ_l[0] = 1.0;

    cartesian_waypoints_planner_B.TJ_l[5] = 1.0;

    cartesian_waypoints_planner_B.TJ_l[10] = 1.0;

    cartesian_waypoints_planner_B.TJ_l[15] = 1.0;

    break;



   case 1:

    ca_rigidBodyJoint_get_JointAxis(obj, cartesian_waypoints_planner_B.v_f);

    cartesian_waypoints_planner_B.axang_idx_0 =

      cartesian_waypoints_planner_B.v_f[0];

    cartesian_waypoints_planner_B.axang_idx_1 =

      cartesian_waypoints_planner_B.v_f[1];

    cartesian_waypoints_planner_B.axang_idx_2 =

      cartesian_waypoints_planner_B.v_f[2];

    cartesian_waypoints_planner_B.b_nn = 1.0 / sqrt

      ((cartesian_waypoints_planner_B.axang_idx_0 *

        cartesian_waypoints_planner_B.axang_idx_0 +

        cartesian_waypoints_planner_B.axang_idx_1 *

        cartesian_waypoints_planner_B.axang_idx_1) +

       cartesian_waypoints_planner_B.axang_idx_2 *

       cartesian_waypoints_planner_B.axang_idx_2);

    cartesian_waypoints_planner_B.v_f[0] =

      cartesian_waypoints_planner_B.axang_idx_0 *

      cartesian_waypoints_planner_B.b_nn;

    cartesian_waypoints_planner_B.v_f[1] =

      cartesian_waypoints_planner_B.axang_idx_1 *

      cartesian_waypoints_planner_B.b_nn;

    cartesian_waypoints_planner_B.v_f[2] =

      cartesian_waypoints_planner_B.axang_idx_2 *

      cartesian_waypoints_planner_B.b_nn;

    cartesian_waypoints_planner_B.axang_idx_0 =

      cartesian_waypoints_planner_B.v_f[1] * cartesian_waypoints_planner_B.v_f[0]

      * 0.0;

    cartesian_waypoints_planner_B.axang_idx_1 =

      cartesian_waypoints_planner_B.v_f[2] * cartesian_waypoints_planner_B.v_f[0]

      * 0.0;

    cartesian_waypoints_planner_B.axang_idx_2 =

      cartesian_waypoints_planner_B.v_f[2] * cartesian_waypoints_planner_B.v_f[1]

      * 0.0;

    cartesian_waypoints_planner_cat(cartesian_waypoints_planner_B.v_f[0] *

      cartesian_waypoints_planner_B.v_f[0] * 0.0 + 1.0,

      cartesian_waypoints_planner_B.axang_idx_0 -

      cartesian_waypoints_planner_B.v_f[2] * 0.0,

      cartesian_waypoints_planner_B.axang_idx_1 +

      cartesian_waypoints_planner_B.v_f[1] * 0.0,

      cartesian_waypoints_planner_B.axang_idx_0 +

      cartesian_waypoints_planner_B.v_f[2] * 0.0,

      cartesian_waypoints_planner_B.v_f[1] * cartesian_waypoints_planner_B.v_f[1]

      * 0.0 + 1.0, cartesian_waypoints_planner_B.axang_idx_2 -

      cartesian_waypoints_planner_B.v_f[0] * 0.0,

      cartesian_waypoints_planner_B.axang_idx_1 -

      cartesian_waypoints_planner_B.v_f[1] * 0.0,

      cartesian_waypoints_planner_B.axang_idx_2 +

      cartesian_waypoints_planner_B.v_f[0] * 0.0,

      cartesian_waypoints_planner_B.v_f[2] * cartesian_waypoints_planner_B.v_f[2]

      * 0.0 + 1.0, cartesian_waypoints_planner_B.tempR_f);

    for (cartesian_waypoints_planner_B.b_kstr_g = 0;

         cartesian_waypoints_planner_B.b_kstr_g < 3;

         cartesian_waypoints_planner_B.b_kstr_g++) {

      cartesian_waypoints_planner_B.loop_ub_d =

        cartesian_waypoints_planner_B.b_kstr_g + 1;

      cartesian_waypoints_planner_B.R_ip[cartesian_waypoints_planner_B.loop_ub_d

        - 1] = cartesian_waypoints_planner_B.tempR_f

        [(cartesian_waypoints_planner_B.loop_ub_d - 1) * 3];

      cartesian_waypoints_planner_B.loop_ub_d =

        cartesian_waypoints_planner_B.b_kstr_g + 1;

      cartesian_waypoints_planner_B.R_ip[cartesian_waypoints_planner_B.loop_ub_d

        + 2] = cartesian_waypoints_planner_B.tempR_f

        [(cartesian_waypoints_planner_B.loop_ub_d - 1) * 3 + 1];

      cartesian_waypoints_planner_B.loop_ub_d =

        cartesian_waypoints_planner_B.b_kstr_g + 1;

      cartesian_waypoints_planner_B.R_ip[cartesian_waypoints_planner_B.loop_ub_d

        + 5] = cartesian_waypoints_planner_B.tempR_f

        [(cartesian_waypoints_planner_B.loop_ub_d - 1) * 3 + 2];

    }



    memset(&cartesian_waypoints_planner_B.TJ_l[0], 0, sizeof(real_T) << 4U);

    for (cartesian_waypoints_planner_B.b_kstr_g = 0;

         cartesian_waypoints_planner_B.b_kstr_g < 3;

         cartesian_waypoints_planner_B.b_kstr_g++) {

      cartesian_waypoints_planner_B.loop_ub_d =

        cartesian_waypoints_planner_B.b_kstr_g << 2;

      cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.loop_ub_d]

        = cartesian_waypoints_planner_B.R_ip[3 *

        cartesian_waypoints_planner_B.b_kstr_g];

      cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.loop_ub_d

        + 1] = cartesian_waypoints_planner_B.R_ip[3 *

        cartesian_waypoints_planner_B.b_kstr_g + 1];

      cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.loop_ub_d

        + 2] = cartesian_waypoints_planner_B.R_ip[3 *

        cartesian_waypoints_planner_B.b_kstr_g + 2];

    }



    cartesian_waypoints_planner_B.TJ_l[15] = 1.0;

    break;



   default:

    ca_rigidBodyJoint_get_JointAxis(obj, cartesian_waypoints_planner_B.v_f);

    memset(&cartesian_waypoints_planner_B.tempR_f[0], 0, 9U * sizeof(real_T));

    cartesian_waypoints_planner_B.tempR_f[0] = 1.0;

    cartesian_waypoints_planner_B.tempR_f[4] = 1.0;

    cartesian_waypoints_planner_B.tempR_f[8] = 1.0;

    for (cartesian_waypoints_planner_B.b_kstr_g = 0;

         cartesian_waypoints_planner_B.b_kstr_g < 3;

         cartesian_waypoints_planner_B.b_kstr_g++) {

      cartesian_waypoints_planner_B.loop_ub_d =

        cartesian_waypoints_planner_B.b_kstr_g << 2;

      cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.loop_ub_d]

        = cartesian_waypoints_planner_B.tempR_f[3 *

        cartesian_waypoints_planner_B.b_kstr_g];

      cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.loop_ub_d

        + 1] = cartesian_waypoints_planner_B.tempR_f[3 *

        cartesian_waypoints_planner_B.b_kstr_g + 1];

      cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.loop_ub_d

        + 2] = cartesian_waypoints_planner_B.tempR_f[3 *

        cartesian_waypoints_planner_B.b_kstr_g + 2];

      cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.b_kstr_g

        + 12] =

        cartesian_waypoints_planner_B.v_f[cartesian_waypoints_planner_B.b_kstr_g]

        * 0.0;

    }



    cartesian_waypoints_planner_B.TJ_l[3] = 0.0;

    cartesian_waypoints_planner_B.TJ_l[7] = 0.0;

    cartesian_waypoints_planner_B.TJ_l[11] = 0.0;

    cartesian_waypoints_planner_B.TJ_l[15] = 1.0;

    break;

  }



  for (cartesian_waypoints_planner_B.b_kstr_g = 0;

       cartesian_waypoints_planner_B.b_kstr_g < 16;

       cartesian_waypoints_planner_B.b_kstr_g++) {

    cartesian_waypoints_planner_B.a_j[cartesian_waypoints_planner_B.b_kstr_g] =

      obj->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_g];

  }



  for (cartesian_waypoints_planner_B.b_kstr_g = 0;

       cartesian_waypoints_planner_B.b_kstr_g < 16;

       cartesian_waypoints_planner_B.b_kstr_g++) {

    cartesian_waypoints_planner_B.b_d[cartesian_waypoints_planner_B.b_kstr_g] =

      obj->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_g];

  }



  for (cartesian_waypoints_planner_B.b_kstr_g = 0;

       cartesian_waypoints_planner_B.b_kstr_g < 4;

       cartesian_waypoints_planner_B.b_kstr_g++) {

    for (cartesian_waypoints_planner_B.loop_ub_d = 0;

         cartesian_waypoints_planner_B.loop_ub_d < 4;

         cartesian_waypoints_planner_B.loop_ub_d++) {

      cartesian_waypoints_planner_B.a_tmp_tmp_f =

        cartesian_waypoints_planner_B.loop_ub_d << 2;

      cartesian_waypoints_planner_B.a_tmp_m =

        cartesian_waypoints_planner_B.b_kstr_g +

        cartesian_waypoints_planner_B.a_tmp_tmp_f;

      cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.a_tmp_m] =

        0.0;

      cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.a_tmp_m] +=

        cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.a_tmp_tmp_f]

        * cartesian_waypoints_planner_B.a_j[cartesian_waypoints_planner_B.b_kstr_g];

      cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.a_tmp_m] +=

        cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.a_tmp_tmp_f

        + 1] *

        cartesian_waypoints_planner_B.a_j[cartesian_waypoints_planner_B.b_kstr_g

        + 4];

      cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.a_tmp_m] +=

        cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.a_tmp_tmp_f

        + 2] *

        cartesian_waypoints_planner_B.a_j[cartesian_waypoints_planner_B.b_kstr_g

        + 8];

      cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.a_tmp_m] +=

        cartesian_waypoints_planner_B.TJ_l[cartesian_waypoints_planner_B.a_tmp_tmp_f

        + 3] *

        cartesian_waypoints_planner_B.a_j[cartesian_waypoints_planner_B.b_kstr_g

        + 12];

    }



    for (cartesian_waypoints_planner_B.loop_ub_d = 0;

         cartesian_waypoints_planner_B.loop_ub_d < 4;

         cartesian_waypoints_planner_B.loop_ub_d++) {

      cartesian_waypoints_planner_B.a_tmp_tmp_f =

        cartesian_waypoints_planner_B.loop_ub_d << 2;

      cartesian_waypoints_planner_B.a_tmp_m =

        cartesian_waypoints_planner_B.b_kstr_g +

        cartesian_waypoints_planner_B.a_tmp_tmp_f;

      T[cartesian_waypoints_planner_B.a_tmp_m] = 0.0;

      T[cartesian_waypoints_planner_B.a_tmp_m] +=

        cartesian_waypoints_planner_B.b_d[cartesian_waypoints_planner_B.a_tmp_tmp_f]

        * cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.b_kstr_g];

      T[cartesian_waypoints_planner_B.a_tmp_m] +=

        cartesian_waypoints_planner_B.b_d[cartesian_waypoints_planner_B.a_tmp_tmp_f

        + 1] *

        cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.b_kstr_g

        + 4];

      T[cartesian_waypoints_planner_B.a_tmp_m] +=

        cartesian_waypoints_planner_B.b_d[cartesian_waypoints_planner_B.a_tmp_tmp_f

        + 2] *

        cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.b_kstr_g

        + 8];

      T[cartesian_waypoints_planner_B.a_tmp_m] +=

        cartesian_waypoints_planner_B.b_d[cartesian_waypoints_planner_B.a_tmp_tmp_f

        + 3] *

        cartesian_waypoints_planner_B.a_g[cartesian_waypoints_planner_B.b_kstr_g

        + 12];

    }

  }

}



static void RigidBodyTree_efficientFKAndJac(x_robotics_manip_internal_Rig_T *obj,

  const real_T qv[6], const emxArray_char_T_cartesian_way_T *body1Name, real_T

  T_data[], int32_T T_size[2], emxArray_real_T_cartesian_way_T *Jac)

{

  w_robotics_manip_internal_Rig_T *body1;

  w_robotics_manip_internal_Rig_T *body2;

  emxArray_real_T_cartesian_way_T *kinematicPathIndices;

  c_rigidBodyJoint_cartesian_wa_T *joint;

  emxArray_char_T_cartesian_way_T *body2Name;

  emxArray_real_T_cartesian_way_T *ancestorIndices1;

  emxArray_real_T_cartesian_way_T *ancestorIndices2;

  emxArray_real_T_cartesian_way_T *y;

  emxArray_real_T_cartesian_way_T *b;

  w_robotics_manip_internal_Rig_T *body;

  emxArray_char_T_cartesian_way_T *bname;

  emxArray_real_T_cartesian_way_T *ancestorIndices1_0;

  emxArray_real_T_cartesian_way_T *ancestorIndices2_0;

  static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };



  boolean_T exitg1;

  int32_T exitg2;

  cartesian_waypoi_emxInit_char_T(&body2Name, 2);

  cartesian_waypoints_planner_B.b_kstr = body2Name->size[0] * body2Name->size[1];

  body2Name->size[0] = 1;

  body2Name->size[1] = obj->Base.NameInternal->size[1];

  cartes_emxEnsureCapacity_char_T(body2Name,

    cartesian_waypoints_planner_B.b_kstr);

  cartesian_waypoints_planner_B.loop_ub_h = obj->Base.NameInternal->size[0] *

    obj->Base.NameInternal->size[1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr = 0;

       cartesian_waypoints_planner_B.b_kstr <=

       cartesian_waypoints_planner_B.loop_ub_h;

       cartesian_waypoints_planner_B.b_kstr++) {

    body2Name->data[cartesian_waypoints_planner_B.b_kstr] =

      obj->Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr];

  }



  cartesian_waypoi_emxInit_char_T(&bname, 2);

  cartesian_waypoints_planner_B.bid1 = -1.0;

  cartesian_waypoints_planner_B.b_kstr = bname->size[0] * bname->size[1];

  bname->size[0] = 1;

  bname->size[1] = obj->Base.NameInternal->size[1];

  cartes_emxEnsureCapacity_char_T(bname, cartesian_waypoints_planner_B.b_kstr);

  cartesian_waypoints_planner_B.loop_ub_h = obj->Base.NameInternal->size[0] *

    obj->Base.NameInternal->size[1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr = 0;

       cartesian_waypoints_planner_B.b_kstr <=

       cartesian_waypoints_planner_B.loop_ub_h;

       cartesian_waypoints_planner_B.b_kstr++) {

    bname->data[cartesian_waypoints_planner_B.b_kstr] = obj->

      Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr];

  }



  if (cartesian_waypoints_plan_strcmp(bname, body1Name)) {

    cartesian_waypoints_planner_B.bid1 = 0.0;

  } else {

    cartesian_waypoints_planner_B.qidx_idx_1 = obj->NumBodies;

    cartesian_waypoints_planner_B.b_i_he = 0;

    exitg1 = false;

    while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_he <=

                         static_cast<int32_T>

                         (cartesian_waypoints_planner_B.qidx_idx_1) - 1)) {

      body1 = obj->Bodies[cartesian_waypoints_planner_B.b_i_he];

      cartesian_waypoints_planner_B.b_kstr = bname->size[0] * bname->size[1];

      bname->size[0] = 1;

      bname->size[1] = body1->NameInternal->size[1];

      cartes_emxEnsureCapacity_char_T(bname,

        cartesian_waypoints_planner_B.b_kstr);

      cartesian_waypoints_planner_B.loop_ub_h = body1->NameInternal->size[0] *

        body1->NameInternal->size[1] - 1;

      for (cartesian_waypoints_planner_B.b_kstr = 0;

           cartesian_waypoints_planner_B.b_kstr <=

           cartesian_waypoints_planner_B.loop_ub_h;

           cartesian_waypoints_planner_B.b_kstr++) {

        bname->data[cartesian_waypoints_planner_B.b_kstr] = body1->

          NameInternal->data[cartesian_waypoints_planner_B.b_kstr];

      }



      if (cartesian_waypoints_plan_strcmp(bname, body1Name)) {

        cartesian_waypoints_planner_B.bid1 = static_cast<real_T>

          (cartesian_waypoints_planner_B.b_i_he) + 1.0;

        exitg1 = true;

      } else {

        cartesian_waypoints_planner_B.b_i_he++;

      }

    }

  }



  cartesian_waypoints_planner_B.bid2 = -1.0;

  cartesian_waypoints_planner_B.b_kstr = bname->size[0] * bname->size[1];

  bname->size[0] = 1;

  bname->size[1] = obj->Base.NameInternal->size[1];

  cartes_emxEnsureCapacity_char_T(bname, cartesian_waypoints_planner_B.b_kstr);

  cartesian_waypoints_planner_B.loop_ub_h = obj->Base.NameInternal->size[0] *

    obj->Base.NameInternal->size[1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr = 0;

       cartesian_waypoints_planner_B.b_kstr <=

       cartesian_waypoints_planner_B.loop_ub_h;

       cartesian_waypoints_planner_B.b_kstr++) {

    bname->data[cartesian_waypoints_planner_B.b_kstr] = obj->

      Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr];

  }



  if (cartesian_waypoints_plan_strcmp(bname, body2Name)) {

    cartesian_waypoints_planner_B.bid2 = 0.0;

  } else {

    cartesian_waypoints_planner_B.qidx_idx_1 = obj->NumBodies;

    cartesian_waypoints_planner_B.b_i_he = 0;

    exitg1 = false;

    while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_he <=

                         static_cast<int32_T>

                         (cartesian_waypoints_planner_B.qidx_idx_1) - 1)) {

      body1 = obj->Bodies[cartesian_waypoints_planner_B.b_i_he];

      cartesian_waypoints_planner_B.b_kstr = bname->size[0] * bname->size[1];

      bname->size[0] = 1;

      bname->size[1] = body1->NameInternal->size[1];

      cartes_emxEnsureCapacity_char_T(bname,

        cartesian_waypoints_planner_B.b_kstr);

      cartesian_waypoints_planner_B.loop_ub_h = body1->NameInternal->size[0] *

        body1->NameInternal->size[1] - 1;

      for (cartesian_waypoints_planner_B.b_kstr = 0;

           cartesian_waypoints_planner_B.b_kstr <=

           cartesian_waypoints_planner_B.loop_ub_h;

           cartesian_waypoints_planner_B.b_kstr++) {

        bname->data[cartesian_waypoints_planner_B.b_kstr] = body1->

          NameInternal->data[cartesian_waypoints_planner_B.b_kstr];

      }



      if (cartesian_waypoints_plan_strcmp(bname, body2Name)) {

        cartesian_waypoints_planner_B.bid2 = static_cast<real_T>

          (cartesian_waypoints_planner_B.b_i_he) + 1.0;

        exitg1 = true;

      } else {

        cartesian_waypoints_planner_B.b_i_he++;

      }

    }

  }



  cartesian_waypoi_emxFree_char_T(&bname);

  if (cartesian_waypoints_planner_B.bid1 == 0.0) {

    body1 = &obj->Base;

  } else {

    body1 = obj->Bodies[static_cast<int32_T>(cartesian_waypoints_planner_B.bid1)

      - 1];

  }



  if (cartesian_waypoints_planner_B.bid2 == 0.0) {

    body2 = &obj->Base;

  } else {

    body2 = obj->Bodies[static_cast<int32_T>(cartesian_waypoints_planner_B.bid2)

      - 1];

  }



  cartesian_waypoi_emxInit_real_T(&ancestorIndices1, 2);

  body = body1;

  cartesian_waypoints_planner_B.b_kstr = ancestorIndices1->size[0] *

    ancestorIndices1->size[1];

  ancestorIndices1->size[0] = 1;

  ancestorIndices1->size[1] = static_cast<int32_T>(obj->NumBodies + 1.0);

  cartes_emxEnsureCapacity_real_T(ancestorIndices1,

    cartesian_waypoints_planner_B.b_kstr);

  cartesian_waypoints_planner_B.loop_ub_h = static_cast<int32_T>(obj->NumBodies

    + 1.0) - 1;

  for (cartesian_waypoints_planner_B.b_kstr = 0;

       cartesian_waypoints_planner_B.b_kstr <=

       cartesian_waypoints_planner_B.loop_ub_h;

       cartesian_waypoints_planner_B.b_kstr++) {

    ancestorIndices1->data[cartesian_waypoints_planner_B.b_kstr] = 0.0;

  }



  cartesian_waypoints_planner_B.bid1 = 2.0;

  ancestorIndices1->data[0] = body1->Index;

  while (body->ParentIndex > 0.0) {

    body = obj->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];

    ancestorIndices1->data[static_cast<int32_T>

      (cartesian_waypoints_planner_B.bid1) - 1] = body->Index;

    cartesian_waypoints_planner_B.bid1++;

  }



  if (body->Index > 0.0) {

    ancestorIndices1->data[static_cast<int32_T>

      (cartesian_waypoints_planner_B.bid1) - 1] = body->ParentIndex;

    cartesian_waypoints_planner_B.bid1++;

  }



  cartesian_waypoi_emxInit_real_T(&ancestorIndices1_0, 2);

  cartesian_waypoints_planner_B.loop_ub_h = static_cast<int32_T>

    (cartesian_waypoints_planner_B.bid1 - 1.0);

  cartesian_waypoints_planner_B.b_kstr = ancestorIndices1_0->size[0] *

    ancestorIndices1_0->size[1];

  ancestorIndices1_0->size[0] = 1;

  ancestorIndices1_0->size[1] = cartesian_waypoints_planner_B.loop_ub_h;

  cartes_emxEnsureCapacity_real_T(ancestorIndices1_0,

    cartesian_waypoints_planner_B.b_kstr);

  for (cartesian_waypoints_planner_B.b_kstr = 0;

       cartesian_waypoints_planner_B.b_kstr <

       cartesian_waypoints_planner_B.loop_ub_h;

       cartesian_waypoints_planner_B.b_kstr++) {

    ancestorIndices1_0->data[cartesian_waypoints_planner_B.b_kstr] =

      ancestorIndices1->data[cartesian_waypoints_planner_B.b_kstr];

  }



  cartesian_waypoints_planner_B.b_kstr = ancestorIndices1->size[0] *

    ancestorIndices1->size[1];

  ancestorIndices1->size[0] = 1;

  ancestorIndices1->size[1] = ancestorIndices1_0->size[1];

  cartes_emxEnsureCapacity_real_T(ancestorIndices1,

    cartesian_waypoints_planner_B.b_kstr);

  cartesian_waypoints_planner_B.loop_ub_h = ancestorIndices1_0->size[0] *

    ancestorIndices1_0->size[1];

  for (cartesian_waypoints_planner_B.b_kstr = 0;

       cartesian_waypoints_planner_B.b_kstr <

       cartesian_waypoints_planner_B.loop_ub_h;

       cartesian_waypoints_planner_B.b_kstr++) {

    ancestorIndices1->data[cartesian_waypoints_planner_B.b_kstr] =

      ancestorIndices1_0->data[cartesian_waypoints_planner_B.b_kstr];

  }



  cartesian_waypoi_emxFree_real_T(&ancestorIndices1_0);

  cartesian_waypoi_emxInit_real_T(&ancestorIndices2, 2);

  body = body2;

  cartesian_waypoints_planner_B.b_kstr = ancestorIndices2->size[0] *

    ancestorIndices2->size[1];

  ancestorIndices2->size[0] = 1;

  ancestorIndices2->size[1] = static_cast<int32_T>(obj->NumBodies + 1.0);

  cartes_emxEnsureCapacity_real_T(ancestorIndices2,

    cartesian_waypoints_planner_B.b_kstr);

  cartesian_waypoints_planner_B.loop_ub_h = static_cast<int32_T>(obj->NumBodies

    + 1.0) - 1;

  for (cartesian_waypoints_planner_B.b_kstr = 0;

       cartesian_waypoints_planner_B.b_kstr <=

       cartesian_waypoints_planner_B.loop_ub_h;

       cartesian_waypoints_planner_B.b_kstr++) {

    ancestorIndices2->data[cartesian_waypoints_planner_B.b_kstr] = 0.0;

  }



  cartesian_waypoints_planner_B.bid1 = 2.0;

  ancestorIndices2->data[0] = body2->Index;

  while (body->ParentIndex > 0.0) {

    body = obj->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];

    ancestorIndices2->data[static_cast<int32_T>

      (cartesian_waypoints_planner_B.bid1) - 1] = body->Index;

    cartesian_waypoints_planner_B.bid1++;

  }



  if (body->Index > 0.0) {

    ancestorIndices2->data[static_cast<int32_T>

      (cartesian_waypoints_planner_B.bid1) - 1] = body->ParentIndex;

    cartesian_waypoints_planner_B.bid1++;

  }



  cartesian_waypoi_emxInit_real_T(&ancestorIndices2_0, 2);

  cartesian_waypoints_planner_B.loop_ub_h = static_cast<int32_T>

    (cartesian_waypoints_planner_B.bid1 - 1.0);

  cartesian_waypoints_planner_B.b_kstr = ancestorIndices2_0->size[0] *

    ancestorIndices2_0->size[1];

  ancestorIndices2_0->size[0] = 1;

  ancestorIndices2_0->size[1] = cartesian_waypoints_planner_B.loop_ub_h;

  cartes_emxEnsureCapacity_real_T(ancestorIndices2_0,

    cartesian_waypoints_planner_B.b_kstr);

  for (cartesian_waypoints_planner_B.b_kstr = 0;

       cartesian_waypoints_planner_B.b_kstr <

       cartesian_waypoints_planner_B.loop_ub_h;

       cartesian_waypoints_planner_B.b_kstr++) {

    ancestorIndices2_0->data[cartesian_waypoints_planner_B.b_kstr] =

      ancestorIndices2->data[cartesian_waypoints_planner_B.b_kstr];

  }



  cartesian_waypoints_planner_B.b_kstr = ancestorIndices2->size[0] *

    ancestorIndices2->size[1];

  ancestorIndices2->size[0] = 1;

  ancestorIndices2->size[1] = ancestorIndices2_0->size[1];

  cartes_emxEnsureCapacity_real_T(ancestorIndices2,

    cartesian_waypoints_planner_B.b_kstr);

  cartesian_waypoints_planner_B.loop_ub_h = ancestorIndices2_0->size[0] *

    ancestorIndices2_0->size[1];

  for (cartesian_waypoints_planner_B.b_kstr = 0;

       cartesian_waypoints_planner_B.b_kstr <

       cartesian_waypoints_planner_B.loop_ub_h;

       cartesian_waypoints_planner_B.b_kstr++) {

    ancestorIndices2->data[cartesian_waypoints_planner_B.b_kstr] =

      ancestorIndices2_0->data[cartesian_waypoints_planner_B.b_kstr];

  }



  cartesian_waypoi_emxFree_real_T(&ancestorIndices2_0);

  cartesian_waypoints_planner_B.bid1 = ancestorIndices1->size[1];

  cartesian_waypoints_planner_B.qidx_idx_1 = ancestorIndices2->size[1];

  if (cartesian_waypoints_planner_B.bid1 <

      cartesian_waypoints_planner_B.qidx_idx_1) {

    cartesian_waypoints_planner_B.qidx_idx_1 =

      cartesian_waypoints_planner_B.bid1;

  }



  cartesian_waypoints_planner_B.bid1 = static_cast<int32_T>

    (cartesian_waypoints_planner_B.qidx_idx_1);

  cartesian_waypoints_planner_B.b_i_he = 0;

  exitg1 = false;

  while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_he <=

                       static_cast<int32_T>

                       (cartesian_waypoints_planner_B.qidx_idx_1) - 2)) {

    if (ancestorIndices1->data[static_cast<int32_T>(static_cast<real_T>

         (ancestorIndices1->size[1]) - (static_cast<real_T>

          (cartesian_waypoints_planner_B.b_i_he) + 1.0)) - 1] !=

        ancestorIndices2->data[static_cast<int32_T>(static_cast<real_T>

         (ancestorIndices2->size[1]) - (static_cast<real_T>

          (cartesian_waypoints_planner_B.b_i_he) + 1.0)) - 1]) {

      cartesian_waypoints_planner_B.bid1 = static_cast<real_T>

        (cartesian_waypoints_planner_B.b_i_he) + 1.0;

      exitg1 = true;

    } else {

      cartesian_waypoints_planner_B.b_i_he++;

    }

  }



  cartesian_waypoints_planner_B.qidx_idx_1 = static_cast<real_T>

    (ancestorIndices1->size[1]) - cartesian_waypoints_planner_B.bid1;

  if (1.0 > cartesian_waypoints_planner_B.qidx_idx_1) {

    cartesian_waypoints_planner_B.b_i_he = -1;

  } else {

    cartesian_waypoints_planner_B.b_i_he = static_cast<int32_T>

      (cartesian_waypoints_planner_B.qidx_idx_1) - 1;

  }



  cartesian_waypoints_planner_B.qidx_idx_1 = static_cast<real_T>

    (ancestorIndices2->size[1]) - cartesian_waypoints_planner_B.bid1;

  if (1.0 > cartesian_waypoints_planner_B.qidx_idx_1) {

    cartesian_waypoints_planner_B.j_n = 0;

    cartesian_waypoints_planner_B.jointSign = 1;

    cartesian_waypoints_planner_B.g = -1;

  } else {

    cartesian_waypoints_planner_B.j_n = static_cast<int32_T>

      (cartesian_waypoints_planner_B.qidx_idx_1) - 1;

    cartesian_waypoints_planner_B.jointSign = -1;

    cartesian_waypoints_planner_B.g = 0;

  }



  cartesian_waypoi_emxInit_real_T(&kinematicPathIndices, 2);

  cartesian_waypoints_planner_B.b_kstr = kinematicPathIndices->size[0] *

    kinematicPathIndices->size[1];

  kinematicPathIndices->size[0] = 1;

  cartesian_waypoints_planner_B.loop_ub_h = div_s32_floor

    (cartesian_waypoints_planner_B.g - cartesian_waypoints_planner_B.j_n,

     cartesian_waypoints_planner_B.jointSign);

  kinematicPathIndices->size[1] = (cartesian_waypoints_planner_B.loop_ub_h +

    cartesian_waypoints_planner_B.b_i_he) + 3;

  cartes_emxEnsureCapacity_real_T(kinematicPathIndices,

    cartesian_waypoints_planner_B.b_kstr);

  for (cartesian_waypoints_planner_B.b_kstr = 0;

       cartesian_waypoints_planner_B.b_kstr <=

       cartesian_waypoints_planner_B.b_i_he;

       cartesian_waypoints_planner_B.b_kstr++) {

    kinematicPathIndices->data[cartesian_waypoints_planner_B.b_kstr] =

      ancestorIndices1->data[cartesian_waypoints_planner_B.b_kstr];

  }



  kinematicPathIndices->data[cartesian_waypoints_planner_B.b_i_he + 1] =

    ancestorIndices1->data[static_cast<int32_T>((static_cast<real_T>

    (ancestorIndices1->size[1]) - cartesian_waypoints_planner_B.bid1) + 1.0) - 1];

  cartesian_waypoi_emxFree_real_T(&ancestorIndices1);

  for (cartesian_waypoints_planner_B.b_kstr = 0;

       cartesian_waypoints_planner_B.b_kstr <=

       cartesian_waypoints_planner_B.loop_ub_h;

       cartesian_waypoints_planner_B.b_kstr++) {

    kinematicPathIndices->data[(cartesian_waypoints_planner_B.b_kstr +

      cartesian_waypoints_planner_B.b_i_he) + 2] = ancestorIndices2->

      data[cartesian_waypoints_planner_B.jointSign *

      cartesian_waypoints_planner_B.b_kstr + cartesian_waypoints_planner_B.j_n];

  }



  cartesian_waypoi_emxFree_real_T(&ancestorIndices2);

  memset(&cartesian_waypoints_planner_B.T1[0], 0, sizeof(real_T) << 4U);

  cartesian_waypoints_planner_B.T1[0] = 1.0;

  cartesian_waypoints_planner_B.T1[5] = 1.0;

  cartesian_waypoints_planner_B.T1[10] = 1.0;

  cartesian_waypoints_planner_B.T1[15] = 1.0;

  cartesian_waypoints_planner_B.b_kstr = Jac->size[0] * Jac->size[1];

  Jac->size[0] = 6;

  Jac->size[1] = static_cast<int32_T>(obj->PositionNumber);

  cartes_emxEnsureCapacity_real_T(Jac, cartesian_waypoints_planner_B.b_kstr);

  cartesian_waypoints_planner_B.loop_ub_h = 6 * static_cast<int32_T>

    (obj->PositionNumber) - 1;

  for (cartesian_waypoints_planner_B.b_kstr = 0;

       cartesian_waypoints_planner_B.b_kstr <=

       cartesian_waypoints_planner_B.loop_ub_h;

       cartesian_waypoints_planner_B.b_kstr++) {

    Jac->data[cartesian_waypoints_planner_B.b_kstr] = 0.0;

  }



  cartesian_waypoints_planner_B.j_n = kinematicPathIndices->size[1] - 2;

  cartesian_waypoi_emxInit_real_T(&y, 2);

  cartesian_waypoi_emxInit_real_T(&b, 2);

  if (0 <= cartesian_waypoints_planner_B.j_n) {

    for (cartesian_waypoints_planner_B.b_kstr = 0;

         cartesian_waypoints_planner_B.b_kstr < 5;

         cartesian_waypoints_planner_B.b_kstr++) {

      cartesian_waypoints_planner_B.b_f1[cartesian_waypoints_planner_B.b_kstr] =

        tmp[cartesian_waypoints_planner_B.b_kstr];

    }

  }



  for (cartesian_waypoints_planner_B.b_i_he = 0;

       cartesian_waypoints_planner_B.b_i_he <= cartesian_waypoints_planner_B.j_n;

       cartesian_waypoints_planner_B.b_i_he++) {

    if (kinematicPathIndices->data[cartesian_waypoints_planner_B.b_i_he] != 0.0)

    {

      body1 = obj->Bodies[static_cast<int32_T>(kinematicPathIndices->

        data[cartesian_waypoints_planner_B.b_i_he]) - 1];

    } else {

      body1 = &obj->Base;

    }



    cartesian_waypoints_planner_B.b_kstr = static_cast<int32_T>

      ((static_cast<real_T>(cartesian_waypoints_planner_B.b_i_he) + 1.0) + 1.0)

      - 1;

    if (kinematicPathIndices->data[cartesian_waypoints_planner_B.b_kstr] != 0.0)

    {

      body2 = obj->Bodies[static_cast<int32_T>(kinematicPathIndices->

        data[cartesian_waypoints_planner_B.b_kstr]) - 1];

    } else {

      body2 = &obj->Base;

    }



    cartesian_waypoints_planner_B.nextBodyIsParent = (body2->Index ==

      body1->ParentIndex);

    if (cartesian_waypoints_planner_B.nextBodyIsParent) {

      body2 = body1;

      cartesian_waypoints_planner_B.jointSign = 1;

    } else {

      cartesian_waypoints_planner_B.jointSign = -1;

    }



    joint = body2->JointInternal;

    cartesian_waypoints_planner_B.b_kstr = body2Name->size[0] * body2Name->size

      [1];

    body2Name->size[0] = 1;

    body2Name->size[1] = joint->Type->size[1];

    cartes_emxEnsureCapacity_char_T(body2Name,

      cartesian_waypoints_planner_B.b_kstr);

    cartesian_waypoints_planner_B.loop_ub_h = joint->Type->size[0] * joint->

      Type->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_kstr = 0;

         cartesian_waypoints_planner_B.b_kstr <=

         cartesian_waypoints_planner_B.loop_ub_h;

         cartesian_waypoints_planner_B.b_kstr++) {

      body2Name->data[cartesian_waypoints_planner_B.b_kstr] = joint->Type->

        data[cartesian_waypoints_planner_B.b_kstr];

    }



    cartesian_waypoints_planner_B.b_bool = false;

    if (body2Name->size[1] == 5) {

      cartesian_waypoints_planner_B.b_kstr = 1;

      do {

        exitg2 = 0;

        if (cartesian_waypoints_planner_B.b_kstr - 1 < 5) {

          cartesian_waypoints_planner_B.g = cartesian_waypoints_planner_B.b_kstr

            - 1;

          if (body2Name->data[cartesian_waypoints_planner_B.g] !=

              cartesian_waypoints_planner_B.b_f1[cartesian_waypoints_planner_B.g])

          {

            exitg2 = 1;

          } else {

            cartesian_waypoints_planner_B.b_kstr++;

          }

        } else {

          cartesian_waypoints_planner_B.b_bool = true;

          exitg2 = 1;

        }

      } while (exitg2 == 0);

    }



    if (cartesian_waypoints_planner_B.b_bool) {

      rigidBodyJoint_transformBodyToP(joint, cartesian_waypoints_planner_B.Tc2p);

    } else {

      cartesian_waypoints_planner_B.b_kstr = static_cast<int32_T>(body2->Index);

      cartesian_waypoints_planner_B.bid1 = obj->

        PositionDoFMap[cartesian_waypoints_planner_B.b_kstr - 1];

      cartesian_waypoints_planner_B.qidx_idx_1 = obj->

        PositionDoFMap[cartesian_waypoints_planner_B.b_kstr + 7];

      if (cartesian_waypoints_planner_B.bid1 >

          cartesian_waypoints_planner_B.qidx_idx_1) {

        cartesian_waypoints_planner_B.g = 0;

        cartesian_waypoints_planner_B.b_kstr = -1;

      } else {

        cartesian_waypoints_planner_B.g = static_cast<int32_T>

          (cartesian_waypoints_planner_B.bid1) - 1;

        cartesian_waypoints_planner_B.b_kstr = static_cast<int32_T>

          (cartesian_waypoints_planner_B.qidx_idx_1) - 1;

      }



      cartesian_waypoints_planner_B.loop_ub_h =

        cartesian_waypoints_planner_B.b_kstr - cartesian_waypoints_planner_B.g;

      cartesian_waypoints_planner_B.qv_size =

        cartesian_waypoints_planner_B.loop_ub_h + 1;

      for (cartesian_waypoints_planner_B.b_kstr = 0;

           cartesian_waypoints_planner_B.b_kstr <=

           cartesian_waypoints_planner_B.loop_ub_h;

           cartesian_waypoints_planner_B.b_kstr++) {

        cartesian_waypoints_planner_B.qv_data[cartesian_waypoints_planner_B.b_kstr]

          = qv[cartesian_waypoints_planner_B.g +

          cartesian_waypoints_planner_B.b_kstr];

      }



      rigidBodyJoint_transformBodyT_e(joint,

        cartesian_waypoints_planner_B.qv_data,

        &cartesian_waypoints_planner_B.qv_size,

        cartesian_waypoints_planner_B.Tc2p);

      cartesian_waypoints_planner_B.b_kstr = static_cast<int32_T>(body2->Index);

      cartesian_waypoints_planner_B.bid1 = obj->

        VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr - 1];

      cartesian_waypoints_planner_B.qidx_idx_1 = obj->

        VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr + 7];

      if (cartesian_waypoints_planner_B.nextBodyIsParent) {

        for (cartesian_waypoints_planner_B.b_kstr = 0;

             cartesian_waypoints_planner_B.b_kstr < 16;

             cartesian_waypoints_planner_B.b_kstr++) {

          cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr]

            = joint->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr];

        }

      } else {

        for (cartesian_waypoints_planner_B.b_kstr = 0;

             cartesian_waypoints_planner_B.b_kstr < 16;

             cartesian_waypoints_planner_B.b_kstr++) {

          cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr]

            = joint->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr];

        }



        for (cartesian_waypoints_planner_B.b_kstr = 0;

             cartesian_waypoints_planner_B.b_kstr < 3;

             cartesian_waypoints_planner_B.b_kstr++) {

          cartesian_waypoints_planner_B.R_i[3 *

            cartesian_waypoints_planner_B.b_kstr] =

            cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr];

          cartesian_waypoints_planner_B.R_i[3 *

            cartesian_waypoints_planner_B.b_kstr + 1] =

            cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr

            + 4];

          cartesian_waypoints_planner_B.R_i[3 *

            cartesian_waypoints_planner_B.b_kstr + 2] =

            cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr

            + 8];

        }



        for (cartesian_waypoints_planner_B.b_kstr = 0;

             cartesian_waypoints_planner_B.b_kstr < 9;

             cartesian_waypoints_planner_B.b_kstr++) {

          cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr]

            =

            -cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr];

        }



        for (cartesian_waypoints_planner_B.b_kstr = 0;

             cartesian_waypoints_planner_B.b_kstr < 3;

             cartesian_waypoints_planner_B.b_kstr++) {

          cartesian_waypoints_planner_B.g = cartesian_waypoints_planner_B.b_kstr

            << 2;

          cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g] =

            cartesian_waypoints_planner_B.R_i[3 *

            cartesian_waypoints_planner_B.b_kstr];

          cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g + 1] =

            cartesian_waypoints_planner_B.R_i[3 *

            cartesian_waypoints_planner_B.b_kstr + 1];

          cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g + 2] =

            cartesian_waypoints_planner_B.R_i[3 *

            cartesian_waypoints_planner_B.b_kstr + 2];

          cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr

            + 12] =

            cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr

            + 6] * cartesian_waypoints_planner_B.T1j[14] +

            (cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr

             + 3] * cartesian_waypoints_planner_B.T1j[13] +

             cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr]

             * cartesian_waypoints_planner_B.T1j[12]);

        }



        cartesian_waypoints_planner_B.Tj[3] = 0.0;

        cartesian_waypoints_planner_B.Tj[7] = 0.0;

        cartesian_waypoints_planner_B.Tj[11] = 0.0;

        cartesian_waypoints_planner_B.Tj[15] = 1.0;

      }



      for (cartesian_waypoints_planner_B.b_kstr = 0;

           cartesian_waypoints_planner_B.b_kstr < 4;

           cartesian_waypoints_planner_B.b_kstr++) {

        for (cartesian_waypoints_planner_B.g = 0;

             cartesian_waypoints_planner_B.g < 4;

             cartesian_waypoints_planner_B.g++) {

          cartesian_waypoints_planner_B.loop_ub_h =

            cartesian_waypoints_planner_B.g << 2;

          cartesian_waypoints_planner_B.n_k =

            cartesian_waypoints_planner_B.b_kstr +

            cartesian_waypoints_planner_B.loop_ub_h;

          cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.n_k] =

            0.0;

          cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.n_k] +=

            cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_h]

            * cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr];

          cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.n_k] +=

            cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_h

            + 1] *

            cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr

            + 4];

          cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.n_k] +=

            cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_h

            + 2] *

            cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr

            + 8];

          cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.n_k] +=

            cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_h

            + 3] *

            cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr

            + 12];

        }

      }



      for (cartesian_waypoints_planner_B.b_kstr = 0;

           cartesian_waypoints_planner_B.b_kstr < 3;

           cartesian_waypoints_planner_B.b_kstr++) {

        cartesian_waypoints_planner_B.R_i[3 *

          cartesian_waypoints_planner_B.b_kstr] =

          cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr];

        cartesian_waypoints_planner_B.R_i[3 *

          cartesian_waypoints_planner_B.b_kstr + 1] =

          cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr

          + 4];

        cartesian_waypoints_planner_B.R_i[3 *

          cartesian_waypoints_planner_B.b_kstr + 2] =

          cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr

          + 8];

      }



      for (cartesian_waypoints_planner_B.b_kstr = 0;

           cartesian_waypoints_planner_B.b_kstr < 9;

           cartesian_waypoints_planner_B.b_kstr++) {

        cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr] =

          -cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr];

      }



      for (cartesian_waypoints_planner_B.b_kstr = 0;

           cartesian_waypoints_planner_B.b_kstr < 3;

           cartesian_waypoints_planner_B.b_kstr++) {

        cartesian_waypoints_planner_B.g = cartesian_waypoints_planner_B.b_kstr <<

          2;

        cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g] =

          cartesian_waypoints_planner_B.R_i[3 *

          cartesian_waypoints_planner_B.b_kstr];

        cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g + 1] =

          cartesian_waypoints_planner_B.R_i[3 *

          cartesian_waypoints_planner_B.b_kstr + 1];

        cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g + 2] =

          cartesian_waypoints_planner_B.R_i[3 *

          cartesian_waypoints_planner_B.b_kstr + 2];

        cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.b_kstr +

          12] =

          cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr

          + 6] * cartesian_waypoints_planner_B.T1j[14] +

          (cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr

           + 3] * cartesian_waypoints_planner_B.T1j[13] +

           cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr]

           * cartesian_waypoints_planner_B.T1j[12]);

      }



      cartesian_waypoints_planner_B.Tj[3] = 0.0;

      cartesian_waypoints_planner_B.Tj[7] = 0.0;

      cartesian_waypoints_planner_B.Tj[11] = 0.0;

      cartesian_waypoints_planner_B.Tj[15] = 1.0;

      cartesian_waypoints_planner_B.R_i[0] = 0.0;

      cartesian_waypoints_planner_B.R_i[3] = -cartesian_waypoints_planner_B.Tj

        [14];

      cartesian_waypoints_planner_B.R_i[6] = cartesian_waypoints_planner_B.Tj[13];

      cartesian_waypoints_planner_B.R_i[1] = cartesian_waypoints_planner_B.Tj[14];

      cartesian_waypoints_planner_B.R_i[4] = 0.0;

      cartesian_waypoints_planner_B.R_i[7] = -cartesian_waypoints_planner_B.Tj

        [12];

      cartesian_waypoints_planner_B.R_i[2] = -cartesian_waypoints_planner_B.Tj

        [13];

      cartesian_waypoints_planner_B.R_i[5] = cartesian_waypoints_planner_B.Tj[12];

      cartesian_waypoints_planner_B.R_i[8] = 0.0;

      for (cartesian_waypoints_planner_B.b_kstr = 0;

           cartesian_waypoints_planner_B.b_kstr < 3;

           cartesian_waypoints_planner_B.b_kstr++) {

        for (cartesian_waypoints_planner_B.g = 0;

             cartesian_waypoints_planner_B.g < 3;

             cartesian_waypoints_planner_B.g++) {

          cartesian_waypoints_planner_B.loop_ub_h =

            cartesian_waypoints_planner_B.b_kstr + 3 *

            cartesian_waypoints_planner_B.g;

          cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.loop_ub_h]

            = 0.0;

          cartesian_waypoints_planner_B.n_k = cartesian_waypoints_planner_B.g <<

            2;

          cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.loop_ub_h]

            +=

            cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.n_k] *

            cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr];

          cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.loop_ub_h]

            +=

            cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.n_k +

            1] *

            cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr

            + 3];

          cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.loop_ub_h]

            +=

            cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.n_k +

            2] *

            cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr

            + 6];

          cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 6 *

            cartesian_waypoints_planner_B.b_kstr] =

            cartesian_waypoints_planner_B.Tj

            [(cartesian_waypoints_planner_B.b_kstr << 2) +

            cartesian_waypoints_planner_B.g];

          cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 6 *

            (cartesian_waypoints_planner_B.b_kstr + 3)] = 0.0;

        }

      }



      for (cartesian_waypoints_planner_B.b_kstr = 0;

           cartesian_waypoints_planner_B.b_kstr < 3;

           cartesian_waypoints_planner_B.b_kstr++) {

        cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr

          + 3] = cartesian_waypoints_planner_B.R_l[3 *

          cartesian_waypoints_planner_B.b_kstr];

        cartesian_waypoints_planner_B.g = cartesian_waypoints_planner_B.b_kstr <<

          2;

        cartesian_waypoints_planner_B.loop_ub_h = 6 *

          (cartesian_waypoints_planner_B.b_kstr + 3);

        cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.loop_ub_h

          + 3] =

          cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g];

        cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr

          + 4] = cartesian_waypoints_planner_B.R_l[3 *

          cartesian_waypoints_planner_B.b_kstr + 1];

        cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.loop_ub_h

          + 4] =

          cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g + 1];

        cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr

          + 5] = cartesian_waypoints_planner_B.R_l[3 *

          cartesian_waypoints_planner_B.b_kstr + 2];

        cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.loop_ub_h

          + 5] =

          cartesian_waypoints_planner_B.Tj[cartesian_waypoints_planner_B.g + 2];

      }



      cartesian_waypoints_planner_B.b_kstr = b->size[0] * b->size[1];

      b->size[0] = 6;

      b->size[1] = joint->MotionSubspace->size[1];

      cartes_emxEnsureCapacity_real_T(b, cartesian_waypoints_planner_B.b_kstr);

      cartesian_waypoints_planner_B.loop_ub_h = joint->MotionSubspace->size[0] *

        joint->MotionSubspace->size[1] - 1;

      for (cartesian_waypoints_planner_B.b_kstr = 0;

           cartesian_waypoints_planner_B.b_kstr <=

           cartesian_waypoints_planner_B.loop_ub_h;

           cartesian_waypoints_planner_B.b_kstr++) {

        b->data[cartesian_waypoints_planner_B.b_kstr] = joint->

          MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr];

      }



      cartesian_waypoints_planner_B.n_k = b->size[1] - 1;

      cartesian_waypoints_planner_B.b_kstr = y->size[0] * y->size[1];

      y->size[0] = 6;

      y->size[1] = b->size[1];

      cartes_emxEnsureCapacity_real_T(y, cartesian_waypoints_planner_B.b_kstr);

      for (cartesian_waypoints_planner_B.b_kstr = 0;

           cartesian_waypoints_planner_B.b_kstr <=

           cartesian_waypoints_planner_B.n_k;

           cartesian_waypoints_planner_B.b_kstr++) {

        cartesian_waypoints_planner_B.coffset_tmp =

          cartesian_waypoints_planner_B.b_kstr * 6 - 1;

        for (cartesian_waypoints_planner_B.g = 0;

             cartesian_waypoints_planner_B.g < 6;

             cartesian_waypoints_planner_B.g++) {

          cartesian_waypoints_planner_B.bid2 = 0.0;

          for (cartesian_waypoints_planner_B.loop_ub_h = 0;

               cartesian_waypoints_planner_B.loop_ub_h < 6;

               cartesian_waypoints_planner_B.loop_ub_h++) {

            cartesian_waypoints_planner_B.bid2 +=

              cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.loop_ub_h

              * 6 + cartesian_waypoints_planner_B.g] * b->data

              [(cartesian_waypoints_planner_B.coffset_tmp +

                cartesian_waypoints_planner_B.loop_ub_h) + 1];

          }



          y->data[(cartesian_waypoints_planner_B.coffset_tmp +

                   cartesian_waypoints_planner_B.g) + 1] =

            cartesian_waypoints_planner_B.bid2;

        }

      }



      if (cartesian_waypoints_planner_B.bid1 >

          cartesian_waypoints_planner_B.qidx_idx_1) {

        cartesian_waypoints_planner_B.n_k = 0;

      } else {

        cartesian_waypoints_planner_B.n_k = static_cast<int32_T>

          (cartesian_waypoints_planner_B.bid1) - 1;

      }



      cartesian_waypoints_planner_B.loop_ub_h = y->size[1];

      for (cartesian_waypoints_planner_B.b_kstr = 0;

           cartesian_waypoints_planner_B.b_kstr <

           cartesian_waypoints_planner_B.loop_ub_h;

           cartesian_waypoints_planner_B.b_kstr++) {

        for (cartesian_waypoints_planner_B.g = 0;

             cartesian_waypoints_planner_B.g < 6;

             cartesian_waypoints_planner_B.g++) {

          Jac->data[cartesian_waypoints_planner_B.g + 6 *

            (cartesian_waypoints_planner_B.n_k +

             cartesian_waypoints_planner_B.b_kstr)] = y->data[6 *

            cartesian_waypoints_planner_B.b_kstr +

            cartesian_waypoints_planner_B.g] * static_cast<real_T>

            (cartesian_waypoints_planner_B.jointSign);

        }

      }

    }



    if (cartesian_waypoints_planner_B.nextBodyIsParent) {

      for (cartesian_waypoints_planner_B.b_kstr = 0;

           cartesian_waypoints_planner_B.b_kstr < 4;

           cartesian_waypoints_planner_B.b_kstr++) {

        for (cartesian_waypoints_planner_B.g = 0;

             cartesian_waypoints_planner_B.g < 4;

             cartesian_waypoints_planner_B.g++) {

          cartesian_waypoints_planner_B.loop_ub_h =

            cartesian_waypoints_planner_B.g << 2;

          cartesian_waypoints_planner_B.jointSign =

            cartesian_waypoints_planner_B.b_kstr +

            cartesian_waypoints_planner_B.loop_ub_h;

          cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.jointSign]

            = 0.0;

          cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.jointSign]

            +=

            cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_h]

            * cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr];

          cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.jointSign]

            +=

            cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_h

            + 1] *

            cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr

            + 4];

          cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.jointSign]

            +=

            cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_h

            + 2] *

            cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr

            + 8];

          cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.jointSign]

            +=

            cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.loop_ub_h

            + 3] *

            cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr

            + 12];

        }

      }



      memcpy(&cartesian_waypoints_planner_B.T1[0],

             &cartesian_waypoints_planner_B.T1j[0], sizeof(real_T) << 4U);

    } else {

      for (cartesian_waypoints_planner_B.b_kstr = 0;

           cartesian_waypoints_planner_B.b_kstr < 3;

           cartesian_waypoints_planner_B.b_kstr++) {

        cartesian_waypoints_planner_B.R_i[3 *

          cartesian_waypoints_planner_B.b_kstr] =

          cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr];

        cartesian_waypoints_planner_B.R_i[3 *

          cartesian_waypoints_planner_B.b_kstr + 1] =

          cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr

          + 4];

        cartesian_waypoints_planner_B.R_i[3 *

          cartesian_waypoints_planner_B.b_kstr + 2] =

          cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.b_kstr

          + 8];

      }



      for (cartesian_waypoints_planner_B.b_kstr = 0;

           cartesian_waypoints_planner_B.b_kstr < 9;

           cartesian_waypoints_planner_B.b_kstr++) {

        cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr] =

          -cartesian_waypoints_planner_B.R_i[cartesian_waypoints_planner_B.b_kstr];

      }



      for (cartesian_waypoints_planner_B.b_kstr = 0;

           cartesian_waypoints_planner_B.b_kstr < 3;

           cartesian_waypoints_planner_B.b_kstr++) {

        cartesian_waypoints_planner_B.loop_ub_h =

          cartesian_waypoints_planner_B.b_kstr << 2;

        cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.loop_ub_h]

          = cartesian_waypoints_planner_B.R_i[3 *

          cartesian_waypoints_planner_B.b_kstr];

        cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.loop_ub_h

          + 1] = cartesian_waypoints_planner_B.R_i[3 *

          cartesian_waypoints_planner_B.b_kstr + 1];

        cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.loop_ub_h

          + 2] = cartesian_waypoints_planner_B.R_i[3 *

          cartesian_waypoints_planner_B.b_kstr + 2];

        cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr +

          12] =

          cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr

          + 6] * cartesian_waypoints_planner_B.Tc2p[14] +

          (cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr

           + 3] * cartesian_waypoints_planner_B.Tc2p[13] +

           cartesian_waypoints_planner_B.R_l[cartesian_waypoints_planner_B.b_kstr]

           * cartesian_waypoints_planner_B.Tc2p[12]);

      }



      cartesian_waypoints_planner_B.T1j[3] = 0.0;

      cartesian_waypoints_planner_B.T1j[7] = 0.0;

      cartesian_waypoints_planner_B.T1j[11] = 0.0;

      cartesian_waypoints_planner_B.T1j[15] = 1.0;

      for (cartesian_waypoints_planner_B.b_kstr = 0;

           cartesian_waypoints_planner_B.b_kstr < 4;

           cartesian_waypoints_planner_B.b_kstr++) {

        for (cartesian_waypoints_planner_B.g = 0;

             cartesian_waypoints_planner_B.g < 4;

             cartesian_waypoints_planner_B.g++) {

          cartesian_waypoints_planner_B.jointSign =

            cartesian_waypoints_planner_B.g << 2;

          cartesian_waypoints_planner_B.loop_ub_h =

            cartesian_waypoints_planner_B.b_kstr +

            cartesian_waypoints_planner_B.jointSign;

          cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.loop_ub_h]

            = 0.0;

          cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.loop_ub_h]

            +=

            cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.jointSign]

            * cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr];

          cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.loop_ub_h]

            +=

            cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.jointSign

            + 1] *

            cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr

            + 4];

          cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.loop_ub_h]

            +=

            cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.jointSign

            + 2] *

            cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr

            + 8];

          cartesian_waypoints_planner_B.Tc2p[cartesian_waypoints_planner_B.loop_ub_h]

            +=

            cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.jointSign

            + 3] *

            cartesian_waypoints_planner_B.T1j[cartesian_waypoints_planner_B.b_kstr

            + 12];

        }

      }



      memcpy(&cartesian_waypoints_planner_B.T1[0],

             &cartesian_waypoints_planner_B.Tc2p[0], sizeof(real_T) << 4U);

    }

  }



  cartesian_waypoi_emxFree_real_T(&b);

  cartesian_waypoi_emxFree_char_T(&body2Name);

  cartesian_waypoi_emxFree_real_T(&kinematicPathIndices);

  for (cartesian_waypoints_planner_B.b_kstr = 0;

       cartesian_waypoints_planner_B.b_kstr < 3;

       cartesian_waypoints_planner_B.b_kstr++) {

    cartesian_waypoints_planner_B.b_i_he = cartesian_waypoints_planner_B.b_kstr <<

      2;

    cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr] =

      cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.b_i_he];

    cartesian_waypoints_planner_B.g = 6 * (cartesian_waypoints_planner_B.b_kstr

      + 3);

    cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g] = 0.0;

    cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr + 3]

      = 0.0;

    cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 3] =

      cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.b_i_he];

    cartesian_waypoints_planner_B.bid1 =

      cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.b_i_he + 1];

    cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr + 1]

      = cartesian_waypoints_planner_B.bid1;

    cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 1] = 0.0;

    cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr + 4]

      = 0.0;

    cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 4] =

      cartesian_waypoints_planner_B.bid1;

    cartesian_waypoints_planner_B.bid1 =

      cartesian_waypoints_planner_B.T1[cartesian_waypoints_planner_B.b_i_he + 2];

    cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr + 2]

      = cartesian_waypoints_planner_B.bid1;

    cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 2] = 0.0;

    cartesian_waypoints_planner_B.X[6 * cartesian_waypoints_planner_B.b_kstr + 5]

      = 0.0;

    cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.g + 5] =

      cartesian_waypoints_planner_B.bid1;

  }



  cartesian_waypoints_planner_B.n_k = Jac->size[1];

  cartesian_waypoints_planner_B.b_kstr = y->size[0] * y->size[1];

  y->size[0] = 6;

  y->size[1] = Jac->size[1];

  cartes_emxEnsureCapacity_real_T(y, cartesian_waypoints_planner_B.b_kstr);

  cartesian_waypoints_planner_B.loop_ub_h = Jac->size[0] * Jac->size[1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr = 0;

       cartesian_waypoints_planner_B.b_kstr <=

       cartesian_waypoints_planner_B.loop_ub_h;

       cartesian_waypoints_planner_B.b_kstr++) {

    y->data[cartesian_waypoints_planner_B.b_kstr] = Jac->

      data[cartesian_waypoints_planner_B.b_kstr];

  }



  cartesian_waypoints_planner_B.b_kstr = Jac->size[0] * Jac->size[1];

  Jac->size[0] = 6;

  Jac->size[1] = cartesian_waypoints_planner_B.n_k;

  cartes_emxEnsureCapacity_real_T(Jac, cartesian_waypoints_planner_B.b_kstr);

  for (cartesian_waypoints_planner_B.b_kstr = 0;

       cartesian_waypoints_planner_B.b_kstr < cartesian_waypoints_planner_B.n_k;

       cartesian_waypoints_planner_B.b_kstr++) {

    cartesian_waypoints_planner_B.coffset_tmp =

      cartesian_waypoints_planner_B.b_kstr * 6 - 1;

    for (cartesian_waypoints_planner_B.b_i_he = 0;

         cartesian_waypoints_planner_B.b_i_he < 6;

         cartesian_waypoints_planner_B.b_i_he++) {

      cartesian_waypoints_planner_B.bid2 = 0.0;

      for (cartesian_waypoints_planner_B.loop_ub_h = 0;

           cartesian_waypoints_planner_B.loop_ub_h < 6;

           cartesian_waypoints_planner_B.loop_ub_h++) {

        cartesian_waypoints_planner_B.bid2 +=

          cartesian_waypoints_planner_B.X[cartesian_waypoints_planner_B.loop_ub_h

          * 6 + cartesian_waypoints_planner_B.b_i_he] * y->data

          [(cartesian_waypoints_planner_B.coffset_tmp +

            cartesian_waypoints_planner_B.loop_ub_h) + 1];

      }



      Jac->data[(cartesian_waypoints_planner_B.coffset_tmp +

                 cartesian_waypoints_planner_B.b_i_he) + 1] =

        cartesian_waypoints_planner_B.bid2;

    }

  }



  cartesian_waypoi_emxFree_real_T(&y);

  T_size[0] = 4;

  T_size[1] = 4;

  memcpy(&T_data[0], &cartesian_waypoints_planner_B.T1[0], sizeof(real_T) << 4U);

}



static real_T cartesian_waypoin_rt_hypotd_snf(real_T u0, real_T u1)

{

  real_T y;

  cartesian_waypoints_planner_B.a_l = fabs(u0);

  y = fabs(u1);

  if (cartesian_waypoints_planner_B.a_l < y) {

    cartesian_waypoints_planner_B.a_l /= y;

    y *= sqrt(cartesian_waypoints_planner_B.a_l *

              cartesian_waypoints_planner_B.a_l + 1.0);

  } else if (cartesian_waypoints_planner_B.a_l > y) {

    y /= cartesian_waypoints_planner_B.a_l;

    y = sqrt(y * y + 1.0) * cartesian_waypoints_planner_B.a_l;

  } else {

    if (!rtIsNaN(y)) {

      y = cartesian_waypoints_planner_B.a_l * 1.4142135623730951;

    }

  }



  return y;

}



static creal_T cartesian_waypoints_planne_sqrt(const creal_T x)

{

  creal_T b_x;

  real_T absxr;

  real_T absxi;

  if (x.im == 0.0) {

    if (x.re < 0.0) {

      absxr = 0.0;

      absxi = sqrt(-x.re);

    } else {

      absxr = sqrt(x.re);

      absxi = 0.0;

    }

  } else if (x.re == 0.0) {

    if (x.im < 0.0) {

      absxr = sqrt(-x.im / 2.0);

      absxi = -absxr;

    } else {

      absxr = sqrt(x.im / 2.0);

      absxi = absxr;

    }

  } else if (rtIsNaN(x.re)) {

    absxr = x.re;

    absxi = x.re;

  } else if (rtIsNaN(x.im)) {

    absxr = x.im;

    absxi = x.im;

  } else if (rtIsInf(x.im)) {

    absxr = fabs(x.im);

    absxi = x.im;

  } else if (rtIsInf(x.re)) {

    if (x.re < 0.0) {

      absxr = 0.0;

      absxi = x.im * -x.re;

    } else {

      absxr = x.re;

      absxi = 0.0;

    }

  } else {

    absxr = fabs(x.re);

    absxi = fabs(x.im);

    if ((absxr > 4.4942328371557893E+307) || (absxi > 4.4942328371557893E+307))

    {

      absxr *= 0.5;

      absxi *= 0.5;

      absxi = cartesian_waypoin_rt_hypotd_snf(absxr, absxi);

      if (absxi > absxr) {

        absxr = sqrt(absxr / absxi + 1.0) * sqrt(absxi);

      } else {

        absxr = sqrt(absxi) * 1.4142135623730951;

      }

    } else {

      absxr = sqrt((cartesian_waypoin_rt_hypotd_snf(absxr, absxi) + absxr) * 0.5);

    }



    if (x.re > 0.0) {

      absxi = x.im / absxr * 0.5;

    } else {

      if (x.im < 0.0) {

        absxi = -absxr;

      } else {

        absxi = absxr;

      }



      absxr = x.im / absxi * 0.5;

    }

  }



  b_x.re = absxr;

  b_x.im = absxi;

  return b_x;

}



real_T rt_atan2d_snf(real_T u0, real_T u1)

{

  real_T y;

  int32_T u0_0;

  int32_T u1_0;

  if (rtIsNaN(u0) || rtIsNaN(u1)) {

    y = (rtNaN);

  } else if (rtIsInf(u0) && rtIsInf(u1)) {

    if (u0 > 0.0) {

      u0_0 = 1;

    } else {

      u0_0 = -1;

    }



    if (u1 > 0.0) {

      u1_0 = 1;

    } else {

      u1_0 = -1;

    }



    y = atan2(static_cast<real_T>(u0_0), static_cast<real_T>(u1_0));

  } else if (u1 == 0.0) {

    if (u0 > 0.0) {

      y = RT_PI / 2.0;

    } else if (u0 < 0.0) {

      y = -(RT_PI / 2.0);

    } else {

      y = 0.0;

    }

  } else {

    y = atan2(u0, u1);

  }



  return y;

}



static real_T cartesian_waypoints_plann_xnrm2(int32_T n, const real_T x[9],

  int32_T ix0)

{

  real_T y;

  real_T scale;

  int32_T kend;

  real_T absxk;

  real_T t;

  int32_T k;

  y = 0.0;

  scale = 3.3121686421112381E-170;

  kend = ix0 + n;

  for (k = ix0; k < kend; k++) {

    absxk = fabs(x[k - 1]);

    if (absxk > scale) {

      t = scale / absxk;

      y = y * t * t + 1.0;

      scale = absxk;

    } else {

      t = absxk / scale;

      y += t * t;

    }

  }



  return scale * sqrt(y);

}



static real_T cartesian_waypoints_plann_xdotc(int32_T n, const real_T x[9],

  int32_T ix0, const real_T y[9], int32_T iy0)

{

  real_T d;

  int32_T ix;

  int32_T iy;

  int32_T k;

  d = 0.0;

  ix = ix0 - 1;

  iy = iy0 - 1;

  for (k = 0; k < n; k++) {

    d += x[ix] * y[iy];

    ix++;

    iy++;

  }



  return d;

}



static void cartesian_waypoints_plann_xaxpy(int32_T n, real_T a, int32_T ix0,

  const real_T y[9], int32_T iy0, real_T b_y[9])

{

  int32_T ix;

  int32_T iy;

  int32_T k;

  memcpy(&b_y[0], &y[0], 9U * sizeof(real_T));

  if (!(a == 0.0)) {

    ix = ix0;

    iy = iy0 - 1;

    for (k = 0; k < n; k++) {

      b_y[iy] += b_y[ix - 1] * a;

      ix++;

      iy++;

    }

  }

}



static real_T cartesian_waypoints_pla_xnrm2_e(const real_T x[3], int32_T ix0)

{

  real_T y;

  real_T scale;

  real_T absxk;

  real_T t;

  int32_T k;

  y = 0.0;

  scale = 3.3121686421112381E-170;

  for (k = ix0; k <= ix0 + 1; k++) {

    absxk = fabs(x[k - 1]);

    if (absxk > scale) {

      t = scale / absxk;

      y = y * t * t + 1.0;

      scale = absxk;

    } else {

      t = absxk / scale;

      y += t * t;

    }

  }



  return scale * sqrt(y);

}



static void cartesian_waypoints_p_xaxpy_evq(int32_T n, real_T a, const real_T x

  [9], int32_T ix0, real_T y[3], int32_T iy0)

{

  int32_T ix;

  int32_T iy;

  int32_T k;

  if (!(a == 0.0)) {

    ix = ix0;

    iy = iy0 - 1;

    for (k = 0; k < n; k++) {

      y[iy] += x[ix - 1] * a;

      ix++;

      iy++;

    }

  }

}



static void cartesian_waypoints_pl_xaxpy_ev(int32_T n, real_T a, const real_T x

  [3], int32_T ix0, const real_T y[9], int32_T iy0, real_T b_y[9])

{

  int32_T ix;

  int32_T iy;

  int32_T k;

  memcpy(&b_y[0], &y[0], 9U * sizeof(real_T));

  if (!(a == 0.0)) {

    ix = ix0;

    iy = iy0 - 1;

    for (k = 0; k < n; k++) {

      b_y[iy] += x[ix - 1] * a;

      ix++;

      iy++;

    }

  }

}



static void cartesian_waypoints_plann_xswap(const real_T x[9], int32_T ix0,

  int32_T iy0, real_T b_x[9])

{

  int32_T ix;

  int32_T iy;

  real_T temp;

  memcpy(&b_x[0], &x[0], 9U * sizeof(real_T));

  ix = ix0 - 1;

  iy = iy0 - 1;

  temp = b_x[ix];

  b_x[ix] = b_x[iy];

  b_x[iy] = temp;

  ix++;

  iy++;

  temp = b_x[ix];

  b_x[ix] = b_x[iy];

  b_x[iy] = temp;

  ix++;

  iy++;

  temp = b_x[ix];

  b_x[ix] = b_x[iy];

  b_x[iy] = temp;

}



static void cartesian_waypoints_plann_xrotg(real_T a, real_T b, real_T *b_a,

  real_T *b_b, real_T *c, real_T *s)

{

  cartesian_waypoints_planner_B.roe = b;

  cartesian_waypoints_planner_B.absa = fabs(a);

  cartesian_waypoints_planner_B.absb = fabs(b);

  if (cartesian_waypoints_planner_B.absa > cartesian_waypoints_planner_B.absb) {

    cartesian_waypoints_planner_B.roe = a;

  }



  cartesian_waypoints_planner_B.scale = cartesian_waypoints_planner_B.absa +

    cartesian_waypoints_planner_B.absb;

  if (cartesian_waypoints_planner_B.scale == 0.0) {

    *s = 0.0;

    *c = 1.0;

    *b_a = 0.0;

    *b_b = 0.0;

  } else {

    cartesian_waypoints_planner_B.ads = cartesian_waypoints_planner_B.absa /

      cartesian_waypoints_planner_B.scale;

    cartesian_waypoints_planner_B.bds = cartesian_waypoints_planner_B.absb /

      cartesian_waypoints_planner_B.scale;

    *b_a = sqrt(cartesian_waypoints_planner_B.ads *

                cartesian_waypoints_planner_B.ads +

                cartesian_waypoints_planner_B.bds *

                cartesian_waypoints_planner_B.bds) *

      cartesian_waypoints_planner_B.scale;

    if (cartesian_waypoints_planner_B.roe < 0.0) {

      *b_a = -*b_a;

    }



    *c = a / *b_a;

    *s = b / *b_a;

    if (cartesian_waypoints_planner_B.absa > cartesian_waypoints_planner_B.absb)

    {

      *b_b = *s;

    } else if (*c != 0.0) {

      *b_b = 1.0 / *c;

    } else {

      *b_b = 1.0;

    }

  }

}



static void cartesian_waypoints_planne_xrot(const real_T x[9], int32_T ix0,

  int32_T iy0, real_T c, real_T s, real_T b_x[9])

{

  int32_T ix;

  int32_T iy;

  real_T temp;

  memcpy(&b_x[0], &x[0], 9U * sizeof(real_T));

  ix = ix0 - 1;

  iy = iy0 - 1;

  temp = c * b_x[ix] + s * b_x[iy];

  b_x[iy] = c * b_x[iy] - s * b_x[ix];

  b_x[ix] = temp;

  iy++;

  ix++;

  temp = c * b_x[ix] + s * b_x[iy];

  b_x[iy] = c * b_x[iy] - s * b_x[ix];

  b_x[ix] = temp;

  iy++;

  ix++;

  temp = c * b_x[ix] + s * b_x[iy];

  b_x[iy] = c * b_x[iy] - s * b_x[ix];

  b_x[ix] = temp;

}



static void cartesian_waypoints_planner_svd(const real_T A[9], real_T U[9],

  real_T s[3], real_T V[9])

{

  int32_T qq;

  boolean_T apply_transform;

  int32_T qjj;

  int32_T m;

  int32_T kase;

  int32_T c_q;

  int32_T d_k;

  boolean_T exitg1;

  cartesian_waypoints_planner_B.e_m[0] = 0.0;

  cartesian_waypoints_planner_B.work_a[0] = 0.0;

  cartesian_waypoints_planner_B.e_m[1] = 0.0;

  cartesian_waypoints_planner_B.work_a[1] = 0.0;

  cartesian_waypoints_planner_B.e_m[2] = 0.0;

  cartesian_waypoints_planner_B.work_a[2] = 0.0;

  for (m = 0; m < 9; m++) {

    cartesian_waypoints_planner_B.A[m] = A[m];

    U[m] = 0.0;

    V[m] = 0.0;

  }



  apply_transform = false;

  cartesian_waypoints_planner_B.nrm = cartesian_waypoints_plann_xnrm2(3,

    cartesian_waypoints_planner_B.A, 1);

  if (cartesian_waypoints_planner_B.nrm > 0.0) {

    apply_transform = true;

    if (cartesian_waypoints_planner_B.A[0] < 0.0) {

      cartesian_waypoints_planner_B.s[0] = -cartesian_waypoints_planner_B.nrm;

    } else {

      cartesian_waypoints_planner_B.s[0] = cartesian_waypoints_planner_B.nrm;

    }



    if (fabs(cartesian_waypoints_planner_B.s[0]) >= 1.0020841800044864E-292) {

      cartesian_waypoints_planner_B.nrm = 1.0 / cartesian_waypoints_planner_B.s

        [0];

      for (qq = 1; qq < 4; qq++) {

        cartesian_waypoints_planner_B.A[qq - 1] *=

          cartesian_waypoints_planner_B.nrm;

      }

    } else {

      for (qq = 1; qq < 4; qq++) {

        cartesian_waypoints_planner_B.A[qq - 1] /=

          cartesian_waypoints_planner_B.s[0];

      }

    }



    cartesian_waypoints_planner_B.A[0]++;

    cartesian_waypoints_planner_B.s[0] = -cartesian_waypoints_planner_B.s[0];

  } else {

    cartesian_waypoints_planner_B.s[0] = 0.0;

  }



  for (m = 2; m < 4; m++) {

    qjj = (m - 1) * 3 + 1;

    if (apply_transform) {

      memcpy(&cartesian_waypoints_planner_B.A_c[0],

             &cartesian_waypoints_planner_B.A[0], 9U * sizeof(real_T));

      cartesian_waypoints_plann_xaxpy(3, -(cartesian_waypoints_plann_xdotc(3,

        cartesian_waypoints_planner_B.A, 1, cartesian_waypoints_planner_B.A, qjj)

        / cartesian_waypoints_planner_B.A[0]), 1,

        cartesian_waypoints_planner_B.A_c, qjj, cartesian_waypoints_planner_B.A);

    }



    cartesian_waypoints_planner_B.e_m[m - 1] =

      cartesian_waypoints_planner_B.A[qjj - 1];

  }



  for (m = 1; m < 4; m++) {

    U[m - 1] = cartesian_waypoints_planner_B.A[m - 1];

  }



  cartesian_waypoints_planner_B.nrm = cartesian_waypoints_pla_xnrm2_e

    (cartesian_waypoints_planner_B.e_m, 2);

  if (cartesian_waypoints_planner_B.nrm == 0.0) {

    cartesian_waypoints_planner_B.e_m[0] = 0.0;

  } else {

    if (cartesian_waypoints_planner_B.e_m[1] < 0.0) {

      cartesian_waypoints_planner_B.rt = -cartesian_waypoints_planner_B.nrm;

      cartesian_waypoints_planner_B.e_m[0] = -cartesian_waypoints_planner_B.nrm;

    } else {

      cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.nrm;

      cartesian_waypoints_planner_B.e_m[0] = cartesian_waypoints_planner_B.nrm;

    }



    if (fabs(cartesian_waypoints_planner_B.rt) >= 1.0020841800044864E-292) {

      cartesian_waypoints_planner_B.nrm = 1.0 / cartesian_waypoints_planner_B.rt;

      for (qq = 2; qq < 4; qq++) {

        cartesian_waypoints_planner_B.e_m[qq - 1] *=

          cartesian_waypoints_planner_B.nrm;

      }

    } else {

      for (qq = 2; qq < 4; qq++) {

        cartesian_waypoints_planner_B.e_m[qq - 1] /=

          cartesian_waypoints_planner_B.rt;

      }

    }



    cartesian_waypoints_planner_B.e_m[1]++;

    cartesian_waypoints_planner_B.e_m[0] = -cartesian_waypoints_planner_B.e_m[0];

    for (m = 2; m < 4; m++) {

      cartesian_waypoints_planner_B.work_a[m - 1] = 0.0;

    }



    for (m = 2; m < 4; m++) {

      cartesian_waypoints_p_xaxpy_evq(2, cartesian_waypoints_planner_B.e_m[m - 1],

        cartesian_waypoints_planner_B.A, 3 * (m - 1) + 2,

        cartesian_waypoints_planner_B.work_a, 2);

    }



    for (m = 2; m < 4; m++) {

      memcpy(&cartesian_waypoints_planner_B.A_c[0],

             &cartesian_waypoints_planner_B.A[0], 9U * sizeof(real_T));

      cartesian_waypoints_pl_xaxpy_ev(2, -cartesian_waypoints_planner_B.e_m[m -

        1] / cartesian_waypoints_planner_B.e_m[1],

        cartesian_waypoints_planner_B.work_a, 2,

        cartesian_waypoints_planner_B.A_c, (m - 1) * 3 + 2,

        cartesian_waypoints_planner_B.A);

    }

  }



  for (m = 2; m < 4; m++) {

    V[m - 1] = cartesian_waypoints_planner_B.e_m[m - 1];

  }



  apply_transform = false;

  cartesian_waypoints_planner_B.nrm = cartesian_waypoints_plann_xnrm2(2,

    cartesian_waypoints_planner_B.A, 5);

  if (cartesian_waypoints_planner_B.nrm > 0.0) {

    apply_transform = true;

    if (cartesian_waypoints_planner_B.A[4] < 0.0) {

      cartesian_waypoints_planner_B.s[1] = -cartesian_waypoints_planner_B.nrm;

    } else {

      cartesian_waypoints_planner_B.s[1] = cartesian_waypoints_planner_B.nrm;

    }



    if (fabs(cartesian_waypoints_planner_B.s[1]) >= 1.0020841800044864E-292) {

      cartesian_waypoints_planner_B.nrm = 1.0 / cartesian_waypoints_planner_B.s

        [1];

      for (qq = 5; qq < 7; qq++) {

        cartesian_waypoints_planner_B.A[qq - 1] *=

          cartesian_waypoints_planner_B.nrm;

      }

    } else {

      for (qq = 5; qq < 7; qq++) {

        cartesian_waypoints_planner_B.A[qq - 1] /=

          cartesian_waypoints_planner_B.s[1];

      }

    }



    cartesian_waypoints_planner_B.A[4]++;

    cartesian_waypoints_planner_B.s[1] = -cartesian_waypoints_planner_B.s[1];

  } else {

    cartesian_waypoints_planner_B.s[1] = 0.0;

  }



  if (apply_transform) {

    for (m = 3; m < 4; m++) {

      memcpy(&cartesian_waypoints_planner_B.A_c[0],

             &cartesian_waypoints_planner_B.A[0], 9U * sizeof(real_T));

      cartesian_waypoints_plann_xaxpy(2, -(cartesian_waypoints_plann_xdotc(2,

        cartesian_waypoints_planner_B.A, 5, cartesian_waypoints_planner_B.A, 8) /

        cartesian_waypoints_planner_B.A[4]), 5,

        cartesian_waypoints_planner_B.A_c, 8, cartesian_waypoints_planner_B.A);

    }

  }



  for (m = 2; m < 4; m++) {

    U[m + 2] = cartesian_waypoints_planner_B.A[m + 2];

  }



  m = 2;

  cartesian_waypoints_planner_B.s[2] = cartesian_waypoints_planner_B.A[8];

  cartesian_waypoints_planner_B.e_m[1] = cartesian_waypoints_planner_B.A[7];

  cartesian_waypoints_planner_B.e_m[2] = 0.0;

  U[6] = 0.0;

  U[7] = 0.0;

  U[8] = 1.0;

  for (c_q = 1; c_q >= 0; c_q--) {

    qq = 3 * c_q + c_q;

    if (cartesian_waypoints_planner_B.s[c_q] != 0.0) {

      for (kase = c_q + 2; kase < 4; kase++) {

        qjj = ((kase - 1) * 3 + c_q) + 1;

        memcpy(&cartesian_waypoints_planner_B.A[0], &U[0], 9U * sizeof(real_T));

        cartesian_waypoints_plann_xaxpy(3 - c_q,

          -(cartesian_waypoints_plann_xdotc(3 - c_q, U, qq + 1, U, qjj) / U[qq]),

          qq + 1, cartesian_waypoints_planner_B.A, qjj, U);

      }



      for (qjj = c_q + 1; qjj < 4; qjj++) {

        kase = (3 * c_q + qjj) - 1;

        U[kase] = -U[kase];

      }



      U[qq]++;

      if (0 <= c_q - 1) {

        U[3 * c_q] = 0.0;

      }

    } else {

      U[3 * c_q] = 0.0;

      U[3 * c_q + 1] = 0.0;

      U[3 * c_q + 2] = 0.0;

      U[qq] = 1.0;

    }

  }



  for (c_q = 2; c_q >= 0; c_q--) {

    if ((c_q + 1 <= 1) && (cartesian_waypoints_planner_B.e_m[0] != 0.0)) {

      memcpy(&cartesian_waypoints_planner_B.A[0], &V[0], 9U * sizeof(real_T));

      cartesian_waypoints_plann_xaxpy(2, -(cartesian_waypoints_plann_xdotc(2, V,

        2, V, 5) / V[1]), 2, cartesian_waypoints_planner_B.A, 5, V);

      memcpy(&cartesian_waypoints_planner_B.A[0], &V[0], 9U * sizeof(real_T));

      cartesian_waypoints_plann_xaxpy(2, -(cartesian_waypoints_plann_xdotc(2, V,

        2, V, 8) / V[1]), 2, cartesian_waypoints_planner_B.A, 8, V);

    }



    V[3 * c_q] = 0.0;

    V[3 * c_q + 1] = 0.0;

    V[3 * c_q + 2] = 0.0;

    V[c_q + 3 * c_q] = 1.0;

  }



  for (c_q = 0; c_q < 3; c_q++) {

    cartesian_waypoints_planner_B.ztest = cartesian_waypoints_planner_B.e_m[c_q];

    if (cartesian_waypoints_planner_B.s[c_q] != 0.0) {

      cartesian_waypoints_planner_B.rt = fabs

        (cartesian_waypoints_planner_B.s[c_q]);

      cartesian_waypoints_planner_B.nrm = cartesian_waypoints_planner_B.s[c_q] /

        cartesian_waypoints_planner_B.rt;

      cartesian_waypoints_planner_B.s[c_q] = cartesian_waypoints_planner_B.rt;

      if (c_q + 1 < 3) {

        cartesian_waypoints_planner_B.ztest =

          cartesian_waypoints_planner_B.e_m[c_q] /

          cartesian_waypoints_planner_B.nrm;

      }



      qjj = 3 * c_q;

      for (qq = qjj + 1; qq <= qjj + 3; qq++) {

        U[qq - 1] *= cartesian_waypoints_planner_B.nrm;

      }

    }



    if ((c_q + 1 < 3) && (cartesian_waypoints_planner_B.ztest != 0.0)) {

      cartesian_waypoints_planner_B.rt = fabs

        (cartesian_waypoints_planner_B.ztest);

      cartesian_waypoints_planner_B.nrm = cartesian_waypoints_planner_B.rt /

        cartesian_waypoints_planner_B.ztest;

      cartesian_waypoints_planner_B.ztest = cartesian_waypoints_planner_B.rt;

      cartesian_waypoints_planner_B.s[c_q + 1] *=

        cartesian_waypoints_planner_B.nrm;

      qjj = (c_q + 1) * 3;

      for (qq = qjj + 1; qq <= qjj + 3; qq++) {

        V[qq - 1] *= cartesian_waypoints_planner_B.nrm;

      }

    }



    cartesian_waypoints_planner_B.e_m[c_q] = cartesian_waypoints_planner_B.ztest;

  }



  qq = 0;

  cartesian_waypoints_planner_B.nrm = 0.0;

  cartesian_waypoints_planner_B.ztest = fabs(cartesian_waypoints_planner_B.s[0]);

  cartesian_waypoints_planner_B.rt = fabs(cartesian_waypoints_planner_B.e_m[0]);

  if ((cartesian_waypoints_planner_B.ztest > cartesian_waypoints_planner_B.rt) ||

      rtIsNaN(cartesian_waypoints_planner_B.rt)) {

    cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.ztest;

  }



  if (!rtIsNaN(cartesian_waypoints_planner_B.rt)) {

    cartesian_waypoints_planner_B.nrm = cartesian_waypoints_planner_B.rt;

  }



  cartesian_waypoints_planner_B.ztest = fabs(cartesian_waypoints_planner_B.s[1]);

  cartesian_waypoints_planner_B.rt = fabs(cartesian_waypoints_planner_B.e_m[1]);

  if ((cartesian_waypoints_planner_B.ztest > cartesian_waypoints_planner_B.rt) ||

      rtIsNaN(cartesian_waypoints_planner_B.rt)) {

    cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.ztest;

  }



  if ((!(cartesian_waypoints_planner_B.nrm > cartesian_waypoints_planner_B.rt)) &&

      (!rtIsNaN(cartesian_waypoints_planner_B.rt))) {

    cartesian_waypoints_planner_B.nrm = cartesian_waypoints_planner_B.rt;

  }



  cartesian_waypoints_planner_B.ztest = fabs(cartesian_waypoints_planner_B.s[2]);

  cartesian_waypoints_planner_B.rt = fabs(cartesian_waypoints_planner_B.e_m[2]);

  if ((cartesian_waypoints_planner_B.ztest > cartesian_waypoints_planner_B.rt) ||

      rtIsNaN(cartesian_waypoints_planner_B.rt)) {

    cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.ztest;

  }



  if ((!(cartesian_waypoints_planner_B.nrm > cartesian_waypoints_planner_B.rt)) &&

      (!rtIsNaN(cartesian_waypoints_planner_B.rt))) {

    cartesian_waypoints_planner_B.nrm = cartesian_waypoints_planner_B.rt;

  }



  while ((m + 1 > 0) && (!(qq >= 75))) {

    c_q = m;

    qjj = m;

    exitg1 = false;

    while ((!exitg1) && (qjj > -1)) {

      c_q = qjj;

      if (qjj == 0) {

        exitg1 = true;

      } else {

        cartesian_waypoints_planner_B.rt = fabs

          (cartesian_waypoints_planner_B.e_m[qjj - 1]);

        if ((cartesian_waypoints_planner_B.rt <= (fabs

              (cartesian_waypoints_planner_B.s[qjj - 1]) + fabs

              (cartesian_waypoints_planner_B.s[qjj])) * 2.2204460492503131E-16) ||

            (cartesian_waypoints_planner_B.rt <= 1.0020841800044864E-292) ||

            ((qq > 20) && (cartesian_waypoints_planner_B.rt <=

                           2.2204460492503131E-16 *

                           cartesian_waypoints_planner_B.nrm))) {

          cartesian_waypoints_planner_B.e_m[qjj - 1] = 0.0;

          exitg1 = true;

        } else {

          qjj--;

        }

      }

    }



    if (c_q == m) {

      kase = 4;

    } else {

      qjj = m + 1;

      kase = m + 1;

      exitg1 = false;

      while ((!exitg1) && (kase >= c_q)) {

        qjj = kase;

        if (kase == c_q) {

          exitg1 = true;

        } else {

          cartesian_waypoints_planner_B.rt = 0.0;

          if (kase < m + 1) {

            cartesian_waypoints_planner_B.rt = fabs

              (cartesian_waypoints_planner_B.e_m[kase - 1]);

          }



          if (kase > c_q + 1) {

            cartesian_waypoints_planner_B.rt += fabs

              (cartesian_waypoints_planner_B.e_m[kase - 2]);

          }



          cartesian_waypoints_planner_B.ztest = fabs

            (cartesian_waypoints_planner_B.s[kase - 1]);

          if ((cartesian_waypoints_planner_B.ztest <= 2.2204460492503131E-16 *

               cartesian_waypoints_planner_B.rt) ||

              (cartesian_waypoints_planner_B.ztest <= 1.0020841800044864E-292))

          {

            cartesian_waypoints_planner_B.s[kase - 1] = 0.0;

            exitg1 = true;

          } else {

            kase--;

          }

        }

      }



      if (qjj == c_q) {

        kase = 3;

      } else if (m + 1 == qjj) {

        kase = 1;

      } else {

        kase = 2;

        c_q = qjj;

      }

    }



    switch (kase) {

     case 1:

      cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.e_m[m - 1];

      cartesian_waypoints_planner_B.e_m[m - 1] = 0.0;

      for (qjj = m; qjj >= c_q + 1; qjj--) {

        cartesian_waypoints_planner_B.ztest = cartesian_waypoints_planner_B.e_m

          [0];

        cartesian_waypoints_plann_xrotg(cartesian_waypoints_planner_B.s[qjj - 1],

          cartesian_waypoints_planner_B.rt, &cartesian_waypoints_planner_B.s[qjj

          - 1], &cartesian_waypoints_planner_B.rt,

          &cartesian_waypoints_planner_B.sqds,

          &cartesian_waypoints_planner_B.b_o);

        if (qjj > c_q + 1) {

          cartesian_waypoints_planner_B.rt = -cartesian_waypoints_planner_B.b_o *

            cartesian_waypoints_planner_B.e_m[0];

          cartesian_waypoints_planner_B.ztest =

            cartesian_waypoints_planner_B.e_m[0] *

            cartesian_waypoints_planner_B.sqds;

        }



        memcpy(&cartesian_waypoints_planner_B.A[0], &V[0], 9U * sizeof(real_T));

        cartesian_waypoints_planne_xrot(cartesian_waypoints_planner_B.A, (qjj -

          1) * 3 + 1, 3 * m + 1, cartesian_waypoints_planner_B.sqds,

          cartesian_waypoints_planner_B.b_o, V);

        cartesian_waypoints_planner_B.e_m[0] =

          cartesian_waypoints_planner_B.ztest;

      }

      break;



     case 2:

      cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.e_m[c_q -

        1];

      cartesian_waypoints_planner_B.e_m[c_q - 1] = 0.0;

      for (qjj = c_q + 1; qjj <= m + 1; qjj++) {

        cartesian_waypoints_plann_xrotg(cartesian_waypoints_planner_B.s[qjj - 1],

          cartesian_waypoints_planner_B.rt, &cartesian_waypoints_planner_B.s[qjj

          - 1], &cartesian_waypoints_planner_B.ztest,

          &cartesian_waypoints_planner_B.sqds,

          &cartesian_waypoints_planner_B.b_o);

        cartesian_waypoints_planner_B.ztest =

          cartesian_waypoints_planner_B.e_m[qjj - 1];

        cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.ztest *

          -cartesian_waypoints_planner_B.b_o;

        cartesian_waypoints_planner_B.e_m[qjj - 1] =

          cartesian_waypoints_planner_B.ztest *

          cartesian_waypoints_planner_B.sqds;

        memcpy(&cartesian_waypoints_planner_B.A[0], &U[0], 9U * sizeof(real_T));

        cartesian_waypoints_planne_xrot(cartesian_waypoints_planner_B.A, (qjj -

          1) * 3 + 1, (c_q - 1) * 3 + 1, cartesian_waypoints_planner_B.sqds,

          cartesian_waypoints_planner_B.b_o, U);

      }

      break;



     case 3:

      cartesian_waypoints_planner_B.ztest = fabs

        (cartesian_waypoints_planner_B.s[m]);

      cartesian_waypoints_planner_B.sqds = cartesian_waypoints_planner_B.s[m - 1];

      cartesian_waypoints_planner_B.rt = fabs(cartesian_waypoints_planner_B.sqds);

      if ((cartesian_waypoints_planner_B.ztest >

           cartesian_waypoints_planner_B.rt) || rtIsNaN

          (cartesian_waypoints_planner_B.rt)) {

        cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.ztest;

      }



      cartesian_waypoints_planner_B.b_o = cartesian_waypoints_planner_B.e_m[m -

        1];

      cartesian_waypoints_planner_B.ztest = fabs

        (cartesian_waypoints_planner_B.b_o);

      if ((cartesian_waypoints_planner_B.rt >

           cartesian_waypoints_planner_B.ztest) || rtIsNaN

          (cartesian_waypoints_planner_B.ztest)) {

        cartesian_waypoints_planner_B.ztest = cartesian_waypoints_planner_B.rt;

      }



      cartesian_waypoints_planner_B.rt = fabs

        (cartesian_waypoints_planner_B.s[c_q]);

      if ((cartesian_waypoints_planner_B.ztest >

           cartesian_waypoints_planner_B.rt) || rtIsNaN

          (cartesian_waypoints_planner_B.rt)) {

        cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.ztest;

      }



      cartesian_waypoints_planner_B.ztest = fabs

        (cartesian_waypoints_planner_B.e_m[c_q]);

      if ((cartesian_waypoints_planner_B.rt >

           cartesian_waypoints_planner_B.ztest) || rtIsNaN

          (cartesian_waypoints_planner_B.ztest)) {

        cartesian_waypoints_planner_B.ztest = cartesian_waypoints_planner_B.rt;

      }



      cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.s[m] /

        cartesian_waypoints_planner_B.ztest;

      cartesian_waypoints_planner_B.smm1 = cartesian_waypoints_planner_B.sqds /

        cartesian_waypoints_planner_B.ztest;

      cartesian_waypoints_planner_B.emm1 = cartesian_waypoints_planner_B.b_o /

        cartesian_waypoints_planner_B.ztest;

      cartesian_waypoints_planner_B.sqds = cartesian_waypoints_planner_B.s[c_q] /

        cartesian_waypoints_planner_B.ztest;

      cartesian_waypoints_planner_B.b_o = ((cartesian_waypoints_planner_B.smm1 +

        cartesian_waypoints_planner_B.rt) * (cartesian_waypoints_planner_B.smm1

        - cartesian_waypoints_planner_B.rt) + cartesian_waypoints_planner_B.emm1

        * cartesian_waypoints_planner_B.emm1) / 2.0;

      cartesian_waypoints_planner_B.smm1 = cartesian_waypoints_planner_B.rt *

        cartesian_waypoints_planner_B.emm1;

      cartesian_waypoints_planner_B.smm1 *= cartesian_waypoints_planner_B.smm1;

      if ((cartesian_waypoints_planner_B.b_o != 0.0) ||

          (cartesian_waypoints_planner_B.smm1 != 0.0)) {

        cartesian_waypoints_planner_B.emm1 = sqrt

          (cartesian_waypoints_planner_B.b_o * cartesian_waypoints_planner_B.b_o

           + cartesian_waypoints_planner_B.smm1);

        if (cartesian_waypoints_planner_B.b_o < 0.0) {

          cartesian_waypoints_planner_B.emm1 =

            -cartesian_waypoints_planner_B.emm1;

        }



        cartesian_waypoints_planner_B.emm1 = cartesian_waypoints_planner_B.smm1 /

          (cartesian_waypoints_planner_B.b_o +

           cartesian_waypoints_planner_B.emm1);

      } else {

        cartesian_waypoints_planner_B.emm1 = 0.0;

      }



      cartesian_waypoints_planner_B.rt = (cartesian_waypoints_planner_B.sqds +

        cartesian_waypoints_planner_B.rt) * (cartesian_waypoints_planner_B.sqds

        - cartesian_waypoints_planner_B.rt) + cartesian_waypoints_planner_B.emm1;

      cartesian_waypoints_planner_B.sqds *=

        cartesian_waypoints_planner_B.e_m[c_q] /

        cartesian_waypoints_planner_B.ztest;

      for (d_k = c_q + 1; d_k <= m; d_k++) {

        cartesian_waypoints_plann_xrotg(cartesian_waypoints_planner_B.rt,

          cartesian_waypoints_planner_B.sqds,

          &cartesian_waypoints_planner_B.ztest,

          &cartesian_waypoints_planner_B.emm1,

          &cartesian_waypoints_planner_B.b_o,

          &cartesian_waypoints_planner_B.smm1);

        if (d_k > c_q + 1) {

          cartesian_waypoints_planner_B.e_m[0] =

            cartesian_waypoints_planner_B.ztest;

        }



        cartesian_waypoints_planner_B.ztest =

          cartesian_waypoints_planner_B.e_m[d_k - 1];

        cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.s[d_k -

          1];

        cartesian_waypoints_planner_B.e_m[d_k - 1] =

          cartesian_waypoints_planner_B.ztest *

          cartesian_waypoints_planner_B.b_o - cartesian_waypoints_planner_B.rt *

          cartesian_waypoints_planner_B.smm1;

        cartesian_waypoints_planner_B.sqds = cartesian_waypoints_planner_B.smm1 *

          cartesian_waypoints_planner_B.s[d_k];

        cartesian_waypoints_planner_B.s[d_k] *=

          cartesian_waypoints_planner_B.b_o;

        qjj = (d_k - 1) * 3 + 1;

        kase = 3 * d_k + 1;

        memcpy(&cartesian_waypoints_planner_B.A[0], &V[0], 9U * sizeof(real_T));

        cartesian_waypoints_planne_xrot(cartesian_waypoints_planner_B.A, qjj,

          kase, cartesian_waypoints_planner_B.b_o,

          cartesian_waypoints_planner_B.smm1, V);

        cartesian_waypoints_plann_xrotg(cartesian_waypoints_planner_B.rt *

          cartesian_waypoints_planner_B.b_o +

          cartesian_waypoints_planner_B.ztest *

          cartesian_waypoints_planner_B.smm1, cartesian_waypoints_planner_B.sqds,

          &cartesian_waypoints_planner_B.s[d_k - 1],

          &cartesian_waypoints_planner_B.unusedU2,

          &cartesian_waypoints_planner_B.emm1,

          &cartesian_waypoints_planner_B.d_sn);

        cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.e_m[d_k

          - 1] * cartesian_waypoints_planner_B.emm1 +

          cartesian_waypoints_planner_B.d_sn *

          cartesian_waypoints_planner_B.s[d_k];

        cartesian_waypoints_planner_B.s[d_k] =

          cartesian_waypoints_planner_B.e_m[d_k - 1] *

          -cartesian_waypoints_planner_B.d_sn +

          cartesian_waypoints_planner_B.emm1 *

          cartesian_waypoints_planner_B.s[d_k];

        cartesian_waypoints_planner_B.sqds = cartesian_waypoints_planner_B.d_sn *

          cartesian_waypoints_planner_B.e_m[d_k];

        cartesian_waypoints_planner_B.e_m[d_k] *=

          cartesian_waypoints_planner_B.emm1;

        memcpy(&cartesian_waypoints_planner_B.A[0], &U[0], 9U * sizeof(real_T));

        cartesian_waypoints_planne_xrot(cartesian_waypoints_planner_B.A, qjj,

          kase, cartesian_waypoints_planner_B.emm1,

          cartesian_waypoints_planner_B.d_sn, U);

      }



      cartesian_waypoints_planner_B.e_m[m - 1] =

        cartesian_waypoints_planner_B.rt;

      qq++;

      break;



     default:

      if (cartesian_waypoints_planner_B.s[c_q] < 0.0) {

        cartesian_waypoints_planner_B.s[c_q] =

          -cartesian_waypoints_planner_B.s[c_q];

        qjj = 3 * c_q;

        for (qq = qjj + 1; qq <= qjj + 3; qq++) {

          V[qq - 1] = -V[qq - 1];

        }

      }



      qq = c_q + 1;

      while ((c_q + 1 < 3) && (cartesian_waypoints_planner_B.s[c_q] <

              cartesian_waypoints_planner_B.s[qq])) {

        cartesian_waypoints_planner_B.rt = cartesian_waypoints_planner_B.s[c_q];

        cartesian_waypoints_planner_B.s[c_q] =

          cartesian_waypoints_planner_B.s[qq];

        cartesian_waypoints_planner_B.s[qq] = cartesian_waypoints_planner_B.rt;

        qjj = 3 * c_q + 1;

        kase = (c_q + 1) * 3 + 1;

        memcpy(&cartesian_waypoints_planner_B.A[0], &V[0], 9U * sizeof(real_T));

        cartesian_waypoints_plann_xswap(cartesian_waypoints_planner_B.A, qjj,

          kase, V);

        memcpy(&cartesian_waypoints_planner_B.A[0], &U[0], 9U * sizeof(real_T));

        cartesian_waypoints_plann_xswap(cartesian_waypoints_planner_B.A, qjj,

          kase, U);

        c_q = qq;

        qq++;

      }



      qq = 0;

      m--;

      break;

    }

  }



  s[0] = cartesian_waypoints_planner_B.s[0];

  s[1] = cartesian_waypoints_planner_B.s[1];

  s[2] = cartesian_waypoints_planner_B.s[2];

}



static void cartesian_waypoints__rotm2axang(const real_T R[9], real_T axang[4])

{

  boolean_T e;

  boolean_T p;

  boolean_T rEQ0;

  int32_T loop_ub_tmp;

  boolean_T exitg1;

  cartesian_waypoints_planner_B.u.re = (((R[0] + R[4]) + R[8]) - 1.0) * 0.5;

  if (!(fabs(cartesian_waypoints_planner_B.u.re) > 1.0)) {

    cartesian_waypoints_planner_B.v_p.re = acos

      (cartesian_waypoints_planner_B.u.re);

  } else {

    cartesian_waypoints_planner_B.u_b.re = cartesian_waypoints_planner_B.u.re +

      1.0;

    cartesian_waypoints_planner_B.u_b.im = 0.0;

    cartesian_waypoints_planner_B.dc.re = 1.0 -

      cartesian_waypoints_planner_B.u.re;

    cartesian_waypoints_planner_B.dc.im = 0.0;

    cartesian_waypoints_planner_B.v_p.re = 2.0 * rt_atan2d_snf

      ((cartesian_waypoints_planne_sqrt(cartesian_waypoints_planner_B.dc)).re,

       (cartesian_waypoints_planne_sqrt(cartesian_waypoints_planner_B.u_b)).re);

  }



  cartesian_waypoints_planner_B.a_ph = 2.0 * sin

    (cartesian_waypoints_planner_B.v_p.re);

  cartesian_waypoints_planner_B.v_h[0] = (R[5] - R[7]) /

    cartesian_waypoints_planner_B.a_ph;

  cartesian_waypoints_planner_B.v_h[1] = (R[6] - R[2]) /

    cartesian_waypoints_planner_B.a_ph;

  cartesian_waypoints_planner_B.v_h[2] = (R[1] - R[3]) /

    cartesian_waypoints_planner_B.a_ph;

  if (rtIsNaN(cartesian_waypoints_planner_B.v_p.re) || rtIsInf

      (cartesian_waypoints_planner_B.v_p.re)) {

    cartesian_waypoints_planner_B.a_ph = (rtNaN);

  } else if (cartesian_waypoints_planner_B.v_p.re == 0.0) {

    cartesian_waypoints_planner_B.a_ph = 0.0;

  } else {

    cartesian_waypoints_planner_B.a_ph = fmod

      (cartesian_waypoints_planner_B.v_p.re, 3.1415926535897931);

    rEQ0 = (cartesian_waypoints_planner_B.a_ph == 0.0);

    if (!rEQ0) {

      cartesian_waypoints_planner_B.q_d = fabs

        (cartesian_waypoints_planner_B.v_p.re / 3.1415926535897931);

      rEQ0 = !(fabs(cartesian_waypoints_planner_B.q_d - floor

                    (cartesian_waypoints_planner_B.q_d + 0.5)) >

               2.2204460492503131E-16 * cartesian_waypoints_planner_B.q_d);

    }



    if (rEQ0) {

      cartesian_waypoints_planner_B.a_ph = 0.0;

    } else {

      if (cartesian_waypoints_planner_B.v_p.re < 0.0) {

        cartesian_waypoints_planner_B.a_ph += 3.1415926535897931;

      }

    }

  }



  rEQ0 = (cartesian_waypoints_planner_B.a_ph == 0.0);

  e = true;

  cartesian_waypoints_planner_B.b_k_e = 0;

  exitg1 = false;

  while ((!exitg1) && (cartesian_waypoints_planner_B.b_k_e < 3)) {

    if (!(cartesian_waypoints_planner_B.v_h[cartesian_waypoints_planner_B.b_k_e]

          == 0.0)) {

      e = false;

      exitg1 = true;

    } else {

      cartesian_waypoints_planner_B.b_k_e++;

    }

  }



  if (rEQ0 || e) {

    loop_ub_tmp = (rEQ0 || e);

    cartesian_waypoints_planner_B.loop_ub_j = loop_ub_tmp * 3 - 1;

    if (0 <= cartesian_waypoints_planner_B.loop_ub_j) {

      memset(&cartesian_waypoints_planner_B.vspecial_data[0], 0,

             (cartesian_waypoints_planner_B.loop_ub_j + 1) * sizeof(real_T));

    }



    loop_ub_tmp--;

    for (cartesian_waypoints_planner_B.loop_ub_j = 0;

         cartesian_waypoints_planner_B.loop_ub_j <= loop_ub_tmp;

         cartesian_waypoints_planner_B.loop_ub_j++) {

      memset(&cartesian_waypoints_planner_B.b_I[0], 0, 9U * sizeof(real_T));

      cartesian_waypoints_planner_B.b_I[0] = 1.0;

      cartesian_waypoints_planner_B.b_I[4] = 1.0;

      cartesian_waypoints_planner_B.b_I[8] = 1.0;

      p = true;

      for (cartesian_waypoints_planner_B.b_k_e = 0;

           cartesian_waypoints_planner_B.b_k_e < 9;

           cartesian_waypoints_planner_B.b_k_e++) {

        cartesian_waypoints_planner_B.a_ph =

          cartesian_waypoints_planner_B.b_I[cartesian_waypoints_planner_B.b_k_e]

          - R[cartesian_waypoints_planner_B.b_k_e];

        if (p && ((!rtIsInf(cartesian_waypoints_planner_B.a_ph)) && (!rtIsNaN

              (cartesian_waypoints_planner_B.a_ph)))) {

        } else {

          p = false;

        }



        cartesian_waypoints_planner_B.b_I[cartesian_waypoints_planner_B.b_k_e] =

          cartesian_waypoints_planner_B.a_ph;

      }



      if (p) {

        cartesian_waypoints_planner_svd(cartesian_waypoints_planner_B.b_I,

          cartesian_waypoints_planner_B.b_U,

          cartesian_waypoints_planner_B.vspecial_data,

          cartesian_waypoints_planner_B.V_g);

      } else {

        for (cartesian_waypoints_planner_B.b_k_e = 0;

             cartesian_waypoints_planner_B.b_k_e < 9;

             cartesian_waypoints_planner_B.b_k_e++) {

          cartesian_waypoints_planner_B.V_g[cartesian_waypoints_planner_B.b_k_e]

            = (rtNaN);

        }

      }



      cartesian_waypoints_planner_B.vspecial_data[0] =

        cartesian_waypoints_planner_B.V_g[6];

      cartesian_waypoints_planner_B.vspecial_data[1] =

        cartesian_waypoints_planner_B.V_g[7];

      cartesian_waypoints_planner_B.vspecial_data[2] =

        cartesian_waypoints_planner_B.V_g[8];

    }



    loop_ub_tmp = 0;

    if (rEQ0 || e) {

      for (cartesian_waypoints_planner_B.loop_ub_j = 0;

           cartesian_waypoints_planner_B.loop_ub_j < 1;

           cartesian_waypoints_planner_B.loop_ub_j++) {

        loop_ub_tmp++;

      }

    }



    for (cartesian_waypoints_planner_B.b_k_e = 0;

         cartesian_waypoints_planner_B.b_k_e < loop_ub_tmp;

         cartesian_waypoints_planner_B.b_k_e++) {

      cartesian_waypoints_planner_B.v_h[0] =

        cartesian_waypoints_planner_B.vspecial_data[3 *

        cartesian_waypoints_planner_B.b_k_e];

      cartesian_waypoints_planner_B.v_h[1] =

        cartesian_waypoints_planner_B.vspecial_data[3 *

        cartesian_waypoints_planner_B.b_k_e + 1];

      cartesian_waypoints_planner_B.v_h[2] =

        cartesian_waypoints_planner_B.vspecial_data[3 *

        cartesian_waypoints_planner_B.b_k_e + 2];

    }

  }



  cartesian_waypoints_planner_B.a_ph = 1.0 / sqrt

    ((cartesian_waypoints_planner_B.v_h[0] * cartesian_waypoints_planner_B.v_h[0]

      + cartesian_waypoints_planner_B.v_h[1] *

      cartesian_waypoints_planner_B.v_h[1]) + cartesian_waypoints_planner_B.v_h

     [2] * cartesian_waypoints_planner_B.v_h[2]);

  cartesian_waypoints_planner_B.v_h[0] *= cartesian_waypoints_planner_B.a_ph;

  cartesian_waypoints_planner_B.v_h[1] *= cartesian_waypoints_planner_B.a_ph;

  axang[0] = cartesian_waypoints_planner_B.v_h[0];

  axang[1] = cartesian_waypoints_planner_B.v_h[1];

  axang[2] = cartesian_waypoints_planner_B.v_h[2] *

    cartesian_waypoints_planner_B.a_ph;

  axang[3] = cartesian_waypoints_planner_B.v_p.re;

}



static void cartesian_IKHelpers_computeCost(const real_T x[6],

  f_robotics_manip_internal_IKE_T *args, real_T *cost, real_T W[36],

  emxArray_real_T_cartesian_way_T *Jac, f_robotics_manip_internal_IKE_T **b_args)

{

  x_robotics_manip_internal_Rig_T *treeInternal;

  emxArray_char_T_cartesian_way_T *bodyName;

  emxArray_real_T_cartesian_way_T *J;

  emxArray_real_T_cartesian_way_T *y;

  cartesian_waypoi_emxInit_char_T(&bodyName, 2);

  *b_args = args;

  treeInternal = args->Robot;

  cartesian_waypoints_planner_B.b_j_f = bodyName->size[0] * bodyName->size[1];

  bodyName->size[0] = 1;

  bodyName->size[1] = args->BodyName->size[1];

  cartes_emxEnsureCapacity_char_T(bodyName, cartesian_waypoints_planner_B.b_j_f);

  cartesian_waypoints_planner_B.loop_ub_c = args->BodyName->size[0] *

    args->BodyName->size[1] - 1;

  for (cartesian_waypoints_planner_B.b_j_f = 0;

       cartesian_waypoints_planner_B.b_j_f <=

       cartesian_waypoints_planner_B.loop_ub_c;

       cartesian_waypoints_planner_B.b_j_f++) {

    bodyName->data[cartesian_waypoints_planner_B.b_j_f] = args->BodyName->

      data[cartesian_waypoints_planner_B.b_j_f];

  }



  for (cartesian_waypoints_planner_B.b_j_f = 0;

       cartesian_waypoints_planner_B.b_j_f < 16;

       cartesian_waypoints_planner_B.b_j_f++) {

    cartesian_waypoints_planner_B.Td[cartesian_waypoints_planner_B.b_j_f] =

      args->Tform[cartesian_waypoints_planner_B.b_j_f];

  }



  for (cartesian_waypoints_planner_B.b_j_f = 0;

       cartesian_waypoints_planner_B.b_j_f < 36;

       cartesian_waypoints_planner_B.b_j_f++) {

    W[cartesian_waypoints_planner_B.b_j_f] = args->

      WeightMatrix[cartesian_waypoints_planner_B.b_j_f];

  }



  cartesian_waypoi_emxInit_real_T(&J, 2);

  RigidBodyTree_efficientFKAndJac(treeInternal, x, bodyName,

    cartesian_waypoints_planner_B.T_data, cartesian_waypoints_planner_B.T_size,

    J);

  cartesian_waypoints_planner_B.b_j_f = Jac->size[0] * Jac->size[1];

  Jac->size[0] = 6;

  Jac->size[1] = J->size[1];

  cartes_emxEnsureCapacity_real_T(Jac, cartesian_waypoints_planner_B.b_j_f);

  cartesian_waypoints_planner_B.loop_ub_c = J->size[0] * J->size[1] - 1;

  cartesian_waypoi_emxFree_char_T(&bodyName);

  for (cartesian_waypoints_planner_B.b_j_f = 0;

       cartesian_waypoints_planner_B.b_j_f <=

       cartesian_waypoints_planner_B.loop_ub_c;

       cartesian_waypoints_planner_B.b_j_f++) {

    Jac->data[cartesian_waypoints_planner_B.b_j_f] = -J->

      data[cartesian_waypoints_planner_B.b_j_f];

  }



  cartesian_waypoi_emxFree_real_T(&J);

  for (cartesian_waypoints_planner_B.b_j_f = 0;

       cartesian_waypoints_planner_B.b_j_f < 3;

       cartesian_waypoints_planner_B.b_j_f++) {

    cartesian_waypoints_planner_B.T[3 * cartesian_waypoints_planner_B.b_j_f] =

      cartesian_waypoints_planner_B.T_data[cartesian_waypoints_planner_B.b_j_f];

    cartesian_waypoints_planner_B.n_f = 3 * cartesian_waypoints_planner_B.b_j_f

      + 1;

    cartesian_waypoints_planner_B.T[cartesian_waypoints_planner_B.n_f] =

      cartesian_waypoints_planner_B.T_data[((cartesian_waypoints_planner_B.b_j_f

      + 1) + cartesian_waypoints_planner_B.T_size[0]) - 1];

    cartesian_waypoints_planner_B.boffset_i = 3 *

      cartesian_waypoints_planner_B.b_j_f + 2;

    cartesian_waypoints_planner_B.T[cartesian_waypoints_planner_B.boffset_i] =

      cartesian_waypoints_planner_B.T_data[((cartesian_waypoints_planner_B.b_j_f

      + 1) + (cartesian_waypoints_planner_B.T_size[0] << 1)) - 1];

    for (cartesian_waypoints_planner_B.loop_ub_c = 0;

         cartesian_waypoints_planner_B.loop_ub_c < 3;

         cartesian_waypoints_planner_B.loop_ub_c++) {

      cartesian_waypoints_planner_B.Td_tmp =

        cartesian_waypoints_planner_B.loop_ub_c + 3 *

        cartesian_waypoints_planner_B.b_j_f;

      cartesian_waypoints_planner_B.Td_a[cartesian_waypoints_planner_B.Td_tmp] =

        0.0;

      cartesian_waypoints_planner_B.Td_a[cartesian_waypoints_planner_B.Td_tmp] +=

        cartesian_waypoints_planner_B.T[3 * cartesian_waypoints_planner_B.b_j_f]

        * cartesian_waypoints_planner_B.Td[cartesian_waypoints_planner_B.loop_ub_c];

      cartesian_waypoints_planner_B.Td_a[cartesian_waypoints_planner_B.Td_tmp] +=

        cartesian_waypoints_planner_B.T[cartesian_waypoints_planner_B.n_f] *

        cartesian_waypoints_planner_B.Td[cartesian_waypoints_planner_B.loop_ub_c

        + 4];

      cartesian_waypoints_planner_B.Td_a[cartesian_waypoints_planner_B.Td_tmp] +=

        cartesian_waypoints_planner_B.T[cartesian_waypoints_planner_B.boffset_i]

        * cartesian_waypoints_planner_B.Td[cartesian_waypoints_planner_B.loop_ub_c

        + 8];

    }

  }



  cartesian_waypoints__rotm2axang(cartesian_waypoints_planner_B.Td_a,

    cartesian_waypoints_planner_B.v);

  cartesian_waypoints_planner_B.e[0] = cartesian_waypoints_planner_B.v[3] *

    cartesian_waypoints_planner_B.v[0];

  cartesian_waypoints_planner_B.e[3] = cartesian_waypoints_planner_B.Td[12] -

    cartesian_waypoints_planner_B.T_data[cartesian_waypoints_planner_B.T_size[0]

    * 3];

  cartesian_waypoints_planner_B.e[1] = cartesian_waypoints_planner_B.v[3] *

    cartesian_waypoints_planner_B.v[1];

  cartesian_waypoints_planner_B.e[4] = cartesian_waypoints_planner_B.Td[13] -

    cartesian_waypoints_planner_B.T_data[cartesian_waypoints_planner_B.T_size[0]

    * 3 + 1];

  cartesian_waypoints_planner_B.e[2] = cartesian_waypoints_planner_B.v[3] *

    cartesian_waypoints_planner_B.v[2];

  cartesian_waypoints_planner_B.e[5] = cartesian_waypoints_planner_B.Td[14] -

    cartesian_waypoints_planner_B.T_data[cartesian_waypoints_planner_B.T_size[0]

    * 3 + 2];

  cartesian_waypoints_planner_B.b_j_f = args->ErrTemp->size[0];

  args->ErrTemp->size[0] = 6;

  cartes_emxEnsureCapacity_real_T(args->ErrTemp,

    cartesian_waypoints_planner_B.b_j_f);

  for (cartesian_waypoints_planner_B.b_j_f = 0;

       cartesian_waypoints_planner_B.b_j_f < 6;

       cartesian_waypoints_planner_B.b_j_f++) {

    args->ErrTemp->data[cartesian_waypoints_planner_B.b_j_f] =

      cartesian_waypoints_planner_B.e[cartesian_waypoints_planner_B.b_j_f];

  }



  for (cartesian_waypoints_planner_B.b_j_f = 0;

       cartesian_waypoints_planner_B.b_j_f < 6;

       cartesian_waypoints_planner_B.b_j_f++) {

    cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_f] = 0.0;

    for (cartesian_waypoints_planner_B.loop_ub_c = 0;

         cartesian_waypoints_planner_B.loop_ub_c < 6;

         cartesian_waypoints_planner_B.loop_ub_c++) {

      cartesian_waypoints_planner_B.s_l = W[6 *

        cartesian_waypoints_planner_B.b_j_f +

        cartesian_waypoints_planner_B.loop_ub_c] * (0.5 *

        cartesian_waypoints_planner_B.e[cartesian_waypoints_planner_B.loop_ub_c])

        + cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_f];

      cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_f] =

        cartesian_waypoints_planner_B.s_l;

    }

  }



  cartesian_waypoints_planner_B.s_l = 0.0;

  for (cartesian_waypoints_planner_B.b_j_f = 0;

       cartesian_waypoints_planner_B.b_j_f < 6;

       cartesian_waypoints_planner_B.b_j_f++) {

    cartesian_waypoints_planner_B.s_l +=

      cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_f] *

      cartesian_waypoints_planner_B.e[cartesian_waypoints_planner_B.b_j_f];

  }



  args->CostTemp = cartesian_waypoints_planner_B.s_l;

  for (cartesian_waypoints_planner_B.b_j_f = 0;

       cartesian_waypoints_planner_B.b_j_f < 6;

       cartesian_waypoints_planner_B.b_j_f++) {

    cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_f] = 0.0;

    for (cartesian_waypoints_planner_B.loop_ub_c = 0;

         cartesian_waypoints_planner_B.loop_ub_c < 6;

         cartesian_waypoints_planner_B.loop_ub_c++) {

      cartesian_waypoints_planner_B.s_l = W[6 *

        cartesian_waypoints_planner_B.b_j_f +

        cartesian_waypoints_planner_B.loop_ub_c] *

        cartesian_waypoints_planner_B.e[cartesian_waypoints_planner_B.loop_ub_c]

        + cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_f];

      cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.b_j_f] =

        cartesian_waypoints_planner_B.s_l;

    }

  }



  cartesian_waypoi_emxInit_real_T(&y, 2);

  cartesian_waypoints_planner_B.n_f = Jac->size[1] - 1;

  cartesian_waypoints_planner_B.b_j_f = y->size[0] * y->size[1];

  y->size[0] = 1;

  y->size[1] = Jac->size[1];

  cartes_emxEnsureCapacity_real_T(y, cartesian_waypoints_planner_B.b_j_f);

  for (cartesian_waypoints_planner_B.b_j_f = 0;

       cartesian_waypoints_planner_B.b_j_f <= cartesian_waypoints_planner_B.n_f;

       cartesian_waypoints_planner_B.b_j_f++) {

    cartesian_waypoints_planner_B.boffset_i =

      cartesian_waypoints_planner_B.b_j_f * 6 - 1;

    cartesian_waypoints_planner_B.s_l = 0.0;

    for (cartesian_waypoints_planner_B.loop_ub_c = 0;

         cartesian_waypoints_planner_B.loop_ub_c < 6;

         cartesian_waypoints_planner_B.loop_ub_c++) {

      cartesian_waypoints_planner_B.s_l += Jac->data

        [(cartesian_waypoints_planner_B.boffset_i +

          cartesian_waypoints_planner_B.loop_ub_c) + 1] *

        cartesian_waypoints_planner_B.y[cartesian_waypoints_planner_B.loop_ub_c];

    }



    y->data[cartesian_waypoints_planner_B.b_j_f] =

      cartesian_waypoints_planner_B.s_l;

  }



  cartesian_waypoints_planner_B.b_j_f = args->GradTemp->size[0];

  args->GradTemp->size[0] = y->size[1];

  cartes_emxEnsureCapacity_real_T(args->GradTemp,

    cartesian_waypoints_planner_B.b_j_f);

  cartesian_waypoints_planner_B.loop_ub_c = y->size[1];

  for (cartesian_waypoints_planner_B.b_j_f = 0;

       cartesian_waypoints_planner_B.b_j_f <

       cartesian_waypoints_planner_B.loop_ub_c;

       cartesian_waypoints_planner_B.b_j_f++) {

    args->GradTemp->data[cartesian_waypoints_planner_B.b_j_f] = y->

      data[cartesian_waypoints_planner_B.b_j_f];

  }



  cartesian_waypoi_emxFree_real_T(&y);

  cartesian_waypoints_planner_B.s_l = args->CostTemp;

  *cost = cartesian_waypoints_planner_B.s_l;

}



static void cartesian_waypoints_planner_eye(real_T b_I[36])

{

  int32_T b_k;

  memset(&b_I[0], 0, 36U * sizeof(real_T));

  for (b_k = 0; b_k < 6; b_k++) {

    b_I[b_k + 6 * b_k] = 1.0;

  }

}



static void cartesian_way_emxInit_boolean_T(emxArray_boolean_T_cartesian__T

  **pEmxArray, int32_T numDimensions)

{

  emxArray_boolean_T_cartesian__T *emxArray;

  int32_T i;

  *pEmxArray = (emxArray_boolean_T_cartesian__T *)malloc(sizeof

    (emxArray_boolean_T_cartesian__T));

  emxArray = *pEmxArray;

  emxArray->data = (boolean_T *)NULL;

  emxArray->numDimensions = numDimensions;

  emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);

  emxArray->allocatedSize = 0;

  emxArray->canFreeData = true;

  for (i = 0; i < numDimensions; i++) {

    emxArray->size[i] = 0;

  }

}



static void cartesian_waypo_emxInit_int32_T(emxArray_int32_T_cartesian_wa_T

  **pEmxArray, int32_T numDimensions)

{

  emxArray_int32_T_cartesian_wa_T *emxArray;

  int32_T i;

  *pEmxArray = (emxArray_int32_T_cartesian_wa_T *)malloc(sizeof

    (emxArray_int32_T_cartesian_wa_T));

  emxArray = *pEmxArray;

  emxArray->data = (int32_T *)NULL;

  emxArray->numDimensions = numDimensions;

  emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);

  emxArray->allocatedSize = 0;

  emxArray->canFreeData = true;

  for (i = 0; i < numDimensions; i++) {

    emxArray->size[i] = 0;

  }

}



static void car_emxEnsureCapacity_boolean_T(emxArray_boolean_T_cartesian__T

  *emxArray, int32_T oldNumel)

{

  int32_T newNumel;

  int32_T i;

  void *newData;

  if (oldNumel < 0) {

    oldNumel = 0;

  }



  newNumel = 1;

  for (i = 0; i < emxArray->numDimensions; i++) {

    newNumel *= emxArray->size[i];

  }



  if (newNumel > emxArray->allocatedSize) {

    i = emxArray->allocatedSize;

    if (i < 16) {

      i = 16;

    }



    while (i < newNumel) {

      if (i > 1073741823) {

        i = MAX_int32_T;

      } else {

        i <<= 1;

      }

    }



    newData = calloc(static_cast<uint32_T>(i), sizeof(boolean_T));

    if (emxArray->data != NULL) {

      memcpy(newData, emxArray->data, sizeof(boolean_T) * oldNumel);

      if (emxArray->canFreeData) {

        free(emxArray->data);

      }

    }



    emxArray->data = (boolean_T *)newData;

    emxArray->allocatedSize = i;

    emxArray->canFreeData = true;

  }

}



static void carte_emxEnsureCapacity_int32_T(emxArray_int32_T_cartesian_wa_T

  *emxArray, int32_T oldNumel)

{

  int32_T newNumel;

  int32_T i;

  void *newData;

  if (oldNumel < 0) {

    oldNumel = 0;

  }



  newNumel = 1;

  for (i = 0; i < emxArray->numDimensions; i++) {

    newNumel *= emxArray->size[i];

  }



  if (newNumel > emxArray->allocatedSize) {

    i = emxArray->allocatedSize;

    if (i < 16) {

      i = 16;

    }



    while (i < newNumel) {

      if (i > 1073741823) {

        i = MAX_int32_T;

      } else {

        i <<= 1;

      }

    }



    newData = calloc(static_cast<uint32_T>(i), sizeof(int32_T));

    if (emxArray->data != NULL) {

      memcpy(newData, emxArray->data, sizeof(int32_T) * oldNumel);

      if (emxArray->canFreeData) {

        free(emxArray->data);

      }

    }



    emxArray->data = (int32_T *)newData;

    emxArray->allocatedSize = i;

    emxArray->canFreeData = true;

  }

}



static real_T cartesian_waypoints_plan_norm_e(const real_T x[6])

{

  real_T y;

  real_T scale;

  real_T absxk;

  real_T t;

  int32_T b_k;

  y = 0.0;

  scale = 3.3121686421112381E-170;

  for (b_k = 0; b_k < 6; b_k++) {

    absxk = fabs(x[b_k]);

    if (absxk > scale) {

      t = scale / absxk;

      y = y * t * t + 1.0;

      scale = absxk;

    } else {

      t = absxk / scale;

      y += t * t;

    }

  }



  return scale * sqrt(y);

}



static real_T SystemTimeProvider_getElapsedTi(const

  f_robotics_core_internal_Syst_T *obj)

{

  real_T systemTime;

  systemTime = ctimefun();

  return systemTime - obj->StartTime;

}



static real_T cartesian_waypoints_pl_xnrm2_ev(int32_T n, const

  emxArray_real_T_cartesian_way_T *x, int32_T ix0)

{

  real_T y;

  y = 0.0;

  if (n >= 1) {

    if (n == 1) {

      y = fabs(x->data[ix0 - 1]);

    } else {

      cartesian_waypoints_planner_B.scale_i = 3.3121686421112381E-170;

      cartesian_waypoints_planner_B.kend_e = ix0 + n;

      for (cartesian_waypoints_planner_B.k_b = ix0;

           cartesian_waypoints_planner_B.k_b <

           cartesian_waypoints_planner_B.kend_e;

           cartesian_waypoints_planner_B.k_b++) {

        cartesian_waypoints_planner_B.absxk_f = fabs(x->

          data[cartesian_waypoints_planner_B.k_b - 1]);

        if (cartesian_waypoints_planner_B.absxk_f >

            cartesian_waypoints_planner_B.scale_i) {

          cartesian_waypoints_planner_B.t_j =

            cartesian_waypoints_planner_B.scale_i /

            cartesian_waypoints_planner_B.absxk_f;

          y = y * cartesian_waypoints_planner_B.t_j *

            cartesian_waypoints_planner_B.t_j + 1.0;

          cartesian_waypoints_planner_B.scale_i =

            cartesian_waypoints_planner_B.absxk_f;

        } else {

          cartesian_waypoints_planner_B.t_j =

            cartesian_waypoints_planner_B.absxk_f /

            cartesian_waypoints_planner_B.scale_i;

          y += cartesian_waypoints_planner_B.t_j *

            cartesian_waypoints_planner_B.t_j;

        }

      }



      y = cartesian_waypoints_planner_B.scale_i * sqrt(y);

    }

  }



  return y;

}



static void cartesian_waypoints_plan_qrpf_e(const

  emxArray_real_T_cartesian_way_T *A, int32_T m, int32_T n,

  emxArray_real_T_cartesian_way_T *tau, const emxArray_int32_T_cartesian_wa_T

  *jpvt, emxArray_real_T_cartesian_way_T *b_A, emxArray_int32_T_cartesian_wa_T

  *b_jpvt)

{

  emxArray_real_T_cartesian_way_T *work;

  emxArray_real_T_cartesian_way_T *vn1;

  emxArray_real_T_cartesian_way_T *vn2;

  emxArray_real_T_cartesian_way_T *c_x;

  int32_T exitg1;

  boolean_T exitg2;

  cartesian_waypoints_planner_B.kend = b_jpvt->size[0] * b_jpvt->size[1];

  b_jpvt->size[0] = 1;

  b_jpvt->size[1] = jpvt->size[1];

  carte_emxEnsureCapacity_int32_T(b_jpvt, cartesian_waypoints_planner_B.kend);

  cartesian_waypoints_planner_B.ix_g = jpvt->size[0] * jpvt->size[1] - 1;

  for (cartesian_waypoints_planner_B.kend = 0;

       cartesian_waypoints_planner_B.kend <= cartesian_waypoints_planner_B.ix_g;

       cartesian_waypoints_planner_B.kend++) {

    b_jpvt->data[cartesian_waypoints_planner_B.kend] = jpvt->

      data[cartesian_waypoints_planner_B.kend];

  }



  cartesian_waypoints_planner_B.kend = b_A->size[0] * b_A->size[1];

  b_A->size[0] = A->size[0];

  b_A->size[1] = A->size[1];

  cartes_emxEnsureCapacity_real_T(b_A, cartesian_waypoints_planner_B.kend);

  cartesian_waypoints_planner_B.ix_g = A->size[0] * A->size[1] - 1;

  for (cartesian_waypoints_planner_B.kend = 0;

       cartesian_waypoints_planner_B.kend <= cartesian_waypoints_planner_B.ix_g;

       cartesian_waypoints_planner_B.kend++) {

    b_A->data[cartesian_waypoints_planner_B.kend] = A->

      data[cartesian_waypoints_planner_B.kend];

  }



  cartesian_waypoi_emxInit_real_T(&work, 1);

  cartesian_waypoints_planner_B.ma = A->size[0];

  if (m < n) {

    cartesian_waypoints_planner_B.m_n = m;

  } else {

    cartesian_waypoints_planner_B.m_n = n;

  }



  cartesian_waypoints_planner_B.minmn_h = cartesian_waypoints_planner_B.m_n - 1;

  cartesian_waypoints_planner_B.kend = work->size[0];

  work->size[0] = A->size[1];

  cartes_emxEnsureCapacity_real_T(work, cartesian_waypoints_planner_B.kend);

  cartesian_waypoints_planner_B.ix_g = A->size[1];

  for (cartesian_waypoints_planner_B.kend = 0;

       cartesian_waypoints_planner_B.kend < cartesian_waypoints_planner_B.ix_g;

       cartesian_waypoints_planner_B.kend++) {

    work->data[cartesian_waypoints_planner_B.kend] = 0.0;

  }



  cartesian_waypoi_emxInit_real_T(&vn1, 1);

  cartesian_waypoints_planner_B.kend = vn1->size[0];

  vn1->size[0] = A->size[1];

  cartes_emxEnsureCapacity_real_T(vn1, cartesian_waypoints_planner_B.kend);

  cartesian_waypoints_planner_B.ix_g = A->size[1];

  for (cartesian_waypoints_planner_B.kend = 0;

       cartesian_waypoints_planner_B.kend < cartesian_waypoints_planner_B.ix_g;

       cartesian_waypoints_planner_B.kend++) {

    vn1->data[cartesian_waypoints_planner_B.kend] = 0.0;

  }



  cartesian_waypoi_emxInit_real_T(&vn2, 1);

  cartesian_waypoints_planner_B.kend = vn2->size[0];

  vn2->size[0] = A->size[1];

  cartes_emxEnsureCapacity_real_T(vn2, cartesian_waypoints_planner_B.kend);

  cartesian_waypoints_planner_B.ix_g = A->size[1];

  for (cartesian_waypoints_planner_B.kend = 0;

       cartesian_waypoints_planner_B.kend < cartesian_waypoints_planner_B.ix_g;

       cartesian_waypoints_planner_B.kend++) {

    vn2->data[cartesian_waypoints_planner_B.kend] = 0.0;

  }



  for (cartesian_waypoints_planner_B.m_n = 0; cartesian_waypoints_planner_B.m_n <

       n; cartesian_waypoints_planner_B.m_n++) {

    cartesian_waypoints_planner_B.pvt = cartesian_waypoints_planner_B.m_n *

      cartesian_waypoints_planner_B.ma;

    cartesian_waypoints_planner_B.smax = 0.0;

    if (m >= 1) {

      if (m == 1) {

        cartesian_waypoints_planner_B.smax = fabs(A->

          data[cartesian_waypoints_planner_B.pvt]);

      } else {

        cartesian_waypoints_planner_B.scale_e = 3.3121686421112381E-170;

        cartesian_waypoints_planner_B.kend = cartesian_waypoints_planner_B.pvt +

          m;

        for (cartesian_waypoints_planner_B.itemp =

             cartesian_waypoints_planner_B.pvt + 1;

             cartesian_waypoints_planner_B.itemp <=

             cartesian_waypoints_planner_B.kend;

             cartesian_waypoints_planner_B.itemp++) {

          cartesian_waypoints_planner_B.absxk_b = fabs(A->

            data[cartesian_waypoints_planner_B.itemp - 1]);

          if (cartesian_waypoints_planner_B.absxk_b >

              cartesian_waypoints_planner_B.scale_e) {

            cartesian_waypoints_planner_B.t_a =

              cartesian_waypoints_planner_B.scale_e /

              cartesian_waypoints_planner_B.absxk_b;

            cartesian_waypoints_planner_B.smax =

              cartesian_waypoints_planner_B.smax *

              cartesian_waypoints_planner_B.t_a *

              cartesian_waypoints_planner_B.t_a + 1.0;

            cartesian_waypoints_planner_B.scale_e =

              cartesian_waypoints_planner_B.absxk_b;

          } else {

            cartesian_waypoints_planner_B.t_a =

              cartesian_waypoints_planner_B.absxk_b /

              cartesian_waypoints_planner_B.scale_e;

            cartesian_waypoints_planner_B.smax +=

              cartesian_waypoints_planner_B.t_a *

              cartesian_waypoints_planner_B.t_a;

          }

        }



        cartesian_waypoints_planner_B.smax =

          cartesian_waypoints_planner_B.scale_e * sqrt

          (cartesian_waypoints_planner_B.smax);

      }

    }



    vn1->data[cartesian_waypoints_planner_B.m_n] =

      cartesian_waypoints_planner_B.smax;

    vn2->data[cartesian_waypoints_planner_B.m_n] = vn1->

      data[cartesian_waypoints_planner_B.m_n];

  }



  cartesian_waypoi_emxInit_real_T(&c_x, 2);

  for (cartesian_waypoints_planner_B.m_n = 0; cartesian_waypoints_planner_B.m_n <=

       cartesian_waypoints_planner_B.minmn_h; cartesian_waypoints_planner_B.m_n

       ++) {

    cartesian_waypoints_planner_B.iy_e = cartesian_waypoints_planner_B.m_n *

      cartesian_waypoints_planner_B.ma;

    cartesian_waypoints_planner_B.ii_c = cartesian_waypoints_planner_B.iy_e +

      cartesian_waypoints_planner_B.m_n;

    cartesian_waypoints_planner_B.nmi = n - cartesian_waypoints_planner_B.m_n;

    cartesian_waypoints_planner_B.mmi = (m - cartesian_waypoints_planner_B.m_n)

      - 1;

    if (cartesian_waypoints_planner_B.nmi < 1) {

      cartesian_waypoints_planner_B.kend = 0;

    } else {

      cartesian_waypoints_planner_B.kend = 1;

      if (cartesian_waypoints_planner_B.nmi > 1) {

        cartesian_waypoints_planner_B.ix_g = cartesian_waypoints_planner_B.m_n;

        cartesian_waypoints_planner_B.smax = fabs(vn1->

          data[cartesian_waypoints_planner_B.m_n]);

        for (cartesian_waypoints_planner_B.itemp = 2;

             cartesian_waypoints_planner_B.itemp <=

             cartesian_waypoints_planner_B.nmi;

             cartesian_waypoints_planner_B.itemp++) {

          cartesian_waypoints_planner_B.ix_g++;

          cartesian_waypoints_planner_B.scale_e = fabs(vn1->

            data[cartesian_waypoints_planner_B.ix_g]);

          if (cartesian_waypoints_planner_B.scale_e >

              cartesian_waypoints_planner_B.smax) {

            cartesian_waypoints_planner_B.kend =

              cartesian_waypoints_planner_B.itemp;

            cartesian_waypoints_planner_B.smax =

              cartesian_waypoints_planner_B.scale_e;

          }

        }

      }

    }



    cartesian_waypoints_planner_B.pvt = (cartesian_waypoints_planner_B.m_n +

      cartesian_waypoints_planner_B.kend) - 1;

    if (cartesian_waypoints_planner_B.pvt + 1 !=

        cartesian_waypoints_planner_B.m_n + 1) {

      cartesian_waypoints_planner_B.kend = c_x->size[0] * c_x->size[1];

      c_x->size[0] = b_A->size[0];

      c_x->size[1] = b_A->size[1];

      cartes_emxEnsureCapacity_real_T(c_x, cartesian_waypoints_planner_B.kend);

      cartesian_waypoints_planner_B.ix_g = b_A->size[0] * b_A->size[1] - 1;

      for (cartesian_waypoints_planner_B.kend = 0;

           cartesian_waypoints_planner_B.kend <=

           cartesian_waypoints_planner_B.ix_g;

           cartesian_waypoints_planner_B.kend++) {

        c_x->data[cartesian_waypoints_planner_B.kend] = b_A->

          data[cartesian_waypoints_planner_B.kend];

      }



      cartesian_waypoints_planner_B.ix_g = cartesian_waypoints_planner_B.pvt *

        cartesian_waypoints_planner_B.ma;

      for (cartesian_waypoints_planner_B.itemp = 0;

           cartesian_waypoints_planner_B.itemp < m;

           cartesian_waypoints_planner_B.itemp++) {

        cartesian_waypoints_planner_B.scale_e = c_x->

          data[cartesian_waypoints_planner_B.ix_g];

        c_x->data[cartesian_waypoints_planner_B.ix_g] = c_x->

          data[cartesian_waypoints_planner_B.iy_e];

        c_x->data[cartesian_waypoints_planner_B.iy_e] =

          cartesian_waypoints_planner_B.scale_e;

        cartesian_waypoints_planner_B.ix_g++;

        cartesian_waypoints_planner_B.iy_e++;

      }



      cartesian_waypoints_planner_B.kend = b_A->size[0] * b_A->size[1];

      b_A->size[0] = c_x->size[0];

      b_A->size[1] = c_x->size[1];

      cartes_emxEnsureCapacity_real_T(b_A, cartesian_waypoints_planner_B.kend);

      cartesian_waypoints_planner_B.ix_g = c_x->size[0] * c_x->size[1] - 1;

      for (cartesian_waypoints_planner_B.kend = 0;

           cartesian_waypoints_planner_B.kend <=

           cartesian_waypoints_planner_B.ix_g;

           cartesian_waypoints_planner_B.kend++) {

        b_A->data[cartesian_waypoints_planner_B.kend] = c_x->

          data[cartesian_waypoints_planner_B.kend];

      }



      cartesian_waypoints_planner_B.itemp = b_jpvt->

        data[cartesian_waypoints_planner_B.pvt];

      b_jpvt->data[cartesian_waypoints_planner_B.pvt] = b_jpvt->

        data[cartesian_waypoints_planner_B.m_n];

      b_jpvt->data[cartesian_waypoints_planner_B.m_n] =

        cartesian_waypoints_planner_B.itemp;

      vn1->data[cartesian_waypoints_planner_B.pvt] = vn1->

        data[cartesian_waypoints_planner_B.m_n];

      vn2->data[cartesian_waypoints_planner_B.pvt] = vn2->

        data[cartesian_waypoints_planner_B.m_n];

    }



    if (cartesian_waypoints_planner_B.m_n + 1 < m) {

      cartesian_waypoints_planner_B.pvt = cartesian_waypoints_planner_B.ii_c + 2;

      cartesian_waypoints_planner_B.kend = c_x->size[0] * c_x->size[1];

      c_x->size[0] = b_A->size[0];

      c_x->size[1] = b_A->size[1];

      cartes_emxEnsureCapacity_real_T(c_x, cartesian_waypoints_planner_B.kend);

      cartesian_waypoints_planner_B.ix_g = b_A->size[0] * b_A->size[1] - 1;

      for (cartesian_waypoints_planner_B.kend = 0;

           cartesian_waypoints_planner_B.kend <=

           cartesian_waypoints_planner_B.ix_g;

           cartesian_waypoints_planner_B.kend++) {

        c_x->data[cartesian_waypoints_planner_B.kend] = b_A->

          data[cartesian_waypoints_planner_B.kend];

      }



      cartesian_waypoints_planner_B.smax = b_A->

        data[cartesian_waypoints_planner_B.ii_c];

      tau->data[cartesian_waypoints_planner_B.m_n] = 0.0;

      if (cartesian_waypoints_planner_B.mmi + 1 > 0) {

        cartesian_waypoints_planner_B.scale_e = cartesian_waypoints_pl_xnrm2_ev

          (cartesian_waypoints_planner_B.mmi, b_A,

           cartesian_waypoints_planner_B.ii_c + 2);

        if (cartesian_waypoints_planner_B.scale_e != 0.0) {

          cartesian_waypoints_planner_B.scale_e =

            cartesian_waypoin_rt_hypotd_snf(b_A->

            data[cartesian_waypoints_planner_B.ii_c],

            cartesian_waypoints_planner_B.scale_e);

          if (b_A->data[cartesian_waypoints_planner_B.ii_c] >= 0.0) {

            cartesian_waypoints_planner_B.scale_e =

              -cartesian_waypoints_planner_B.scale_e;

          }



          if (fabs(cartesian_waypoints_planner_B.scale_e) <

              1.0020841800044864E-292) {

            cartesian_waypoints_planner_B.kend = -1;

            cartesian_waypoints_planner_B.ix_g =

              (cartesian_waypoints_planner_B.ii_c +

               cartesian_waypoints_planner_B.mmi) + 1;

            do {

              cartesian_waypoints_planner_B.kend++;

              for (cartesian_waypoints_planner_B.itemp =

                   cartesian_waypoints_planner_B.pvt;

                   cartesian_waypoints_planner_B.itemp <=

                   cartesian_waypoints_planner_B.ix_g;

                   cartesian_waypoints_planner_B.itemp++) {

                c_x->data[cartesian_waypoints_planner_B.itemp - 1] *=

                  9.9792015476736E+291;

              }



              cartesian_waypoints_planner_B.scale_e *= 9.9792015476736E+291;

              cartesian_waypoints_planner_B.smax *= 9.9792015476736E+291;

            } while (!(fabs(cartesian_waypoints_planner_B.scale_e) >=

                       1.0020841800044864E-292));



            cartesian_waypoints_planner_B.scale_e =

              cartesian_waypoin_rt_hypotd_snf(cartesian_waypoints_planner_B.smax,

              cartesian_waypoints_pl_xnrm2_ev(cartesian_waypoints_planner_B.mmi,

              c_x, cartesian_waypoints_planner_B.ii_c + 2));

            if (cartesian_waypoints_planner_B.smax >= 0.0) {

              cartesian_waypoints_planner_B.scale_e =

                -cartesian_waypoints_planner_B.scale_e;

            }



            tau->data[cartesian_waypoints_planner_B.m_n] =

              (cartesian_waypoints_planner_B.scale_e -

               cartesian_waypoints_planner_B.smax) /

              cartesian_waypoints_planner_B.scale_e;

            cartesian_waypoints_planner_B.smax = 1.0 /

              (cartesian_waypoints_planner_B.smax -

               cartesian_waypoints_planner_B.scale_e);

            for (cartesian_waypoints_planner_B.itemp =

                 cartesian_waypoints_planner_B.pvt;

                 cartesian_waypoints_planner_B.itemp <=

                 cartesian_waypoints_planner_B.ix_g;

                 cartesian_waypoints_planner_B.itemp++) {

              c_x->data[cartesian_waypoints_planner_B.itemp - 1] *=

                cartesian_waypoints_planner_B.smax;

            }



            for (cartesian_waypoints_planner_B.itemp = 0;

                 cartesian_waypoints_planner_B.itemp <=

                 cartesian_waypoints_planner_B.kend;

                 cartesian_waypoints_planner_B.itemp++) {

              cartesian_waypoints_planner_B.scale_e *= 1.0020841800044864E-292;

            }



            cartesian_waypoints_planner_B.smax =

              cartesian_waypoints_planner_B.scale_e;

          } else {

            tau->data[cartesian_waypoints_planner_B.m_n] =

              (cartesian_waypoints_planner_B.scale_e - b_A->

               data[cartesian_waypoints_planner_B.ii_c]) /

              cartesian_waypoints_planner_B.scale_e;

            cartesian_waypoints_planner_B.smax = 1.0 / (b_A->

              data[cartesian_waypoints_planner_B.ii_c] -

              cartesian_waypoints_planner_B.scale_e);

            cartesian_waypoints_planner_B.kend = c_x->size[0] * c_x->size[1];

            c_x->size[0] = b_A->size[0];

            c_x->size[1] = b_A->size[1];

            cartes_emxEnsureCapacity_real_T(c_x,

              cartesian_waypoints_planner_B.kend);

            cartesian_waypoints_planner_B.ix_g = b_A->size[0] * b_A->size[1] - 1;

            for (cartesian_waypoints_planner_B.kend = 0;

                 cartesian_waypoints_planner_B.kend <=

                 cartesian_waypoints_planner_B.ix_g;

                 cartesian_waypoints_planner_B.kend++) {

              c_x->data[cartesian_waypoints_planner_B.kend] = b_A->

                data[cartesian_waypoints_planner_B.kend];

            }



            cartesian_waypoints_planner_B.b_fni =

              (cartesian_waypoints_planner_B.ii_c +

               cartesian_waypoints_planner_B.mmi) + 1;

            for (cartesian_waypoints_planner_B.itemp =

                 cartesian_waypoints_planner_B.pvt;

                 cartesian_waypoints_planner_B.itemp <=

                 cartesian_waypoints_planner_B.b_fni;

                 cartesian_waypoints_planner_B.itemp++) {

              c_x->data[cartesian_waypoints_planner_B.itemp - 1] *=

                cartesian_waypoints_planner_B.smax;

            }



            cartesian_waypoints_planner_B.smax =

              cartesian_waypoints_planner_B.scale_e;

          }

        }

      }



      cartesian_waypoints_planner_B.kend = b_A->size[0] * b_A->size[1];

      b_A->size[0] = c_x->size[0];

      b_A->size[1] = c_x->size[1];

      cartes_emxEnsureCapacity_real_T(b_A, cartesian_waypoints_planner_B.kend);

      cartesian_waypoints_planner_B.ix_g = c_x->size[0] * c_x->size[1] - 1;

      for (cartesian_waypoints_planner_B.kend = 0;

           cartesian_waypoints_planner_B.kend <=

           cartesian_waypoints_planner_B.ix_g;

           cartesian_waypoints_planner_B.kend++) {

        b_A->data[cartesian_waypoints_planner_B.kend] = c_x->

          data[cartesian_waypoints_planner_B.kend];

      }



      b_A->data[cartesian_waypoints_planner_B.ii_c] =

        cartesian_waypoints_planner_B.smax;

    } else {

      tau->data[cartesian_waypoints_planner_B.m_n] = 0.0;

    }



    if (cartesian_waypoints_planner_B.m_n + 1 < n) {

      cartesian_waypoints_planner_B.smax = b_A->

        data[cartesian_waypoints_planner_B.ii_c];

      b_A->data[cartesian_waypoints_planner_B.ii_c] = 1.0;

      cartesian_waypoints_planner_B.pvt = (cartesian_waypoints_planner_B.ii_c +

        cartesian_waypoints_planner_B.ma) + 1;

      cartesian_waypoints_planner_B.kend = c_x->size[0] * c_x->size[1];

      c_x->size[0] = b_A->size[0];

      c_x->size[1] = b_A->size[1];

      cartes_emxEnsureCapacity_real_T(c_x, cartesian_waypoints_planner_B.kend);

      cartesian_waypoints_planner_B.ix_g = b_A->size[0] * b_A->size[1] - 1;

      for (cartesian_waypoints_planner_B.kend = 0;

           cartesian_waypoints_planner_B.kend <=

           cartesian_waypoints_planner_B.ix_g;

           cartesian_waypoints_planner_B.kend++) {

        c_x->data[cartesian_waypoints_planner_B.kend] = b_A->

          data[cartesian_waypoints_planner_B.kend];

      }



      if (tau->data[cartesian_waypoints_planner_B.m_n] != 0.0) {

        cartesian_waypoints_planner_B.itemp = cartesian_waypoints_planner_B.mmi;

        cartesian_waypoints_planner_B.kend = cartesian_waypoints_planner_B.ii_c

          + cartesian_waypoints_planner_B.mmi;

        while ((cartesian_waypoints_planner_B.itemp + 1 > 0) && (b_A->

                data[cartesian_waypoints_planner_B.kend] == 0.0)) {

          cartesian_waypoints_planner_B.itemp--;

          cartesian_waypoints_planner_B.kend--;

        }



        cartesian_waypoints_planner_B.nmi--;

        exitg2 = false;

        while ((!exitg2) && (cartesian_waypoints_planner_B.nmi > 0)) {

          cartesian_waypoints_planner_B.ix_g =

            (cartesian_waypoints_planner_B.nmi - 1) *

            cartesian_waypoints_planner_B.ma + cartesian_waypoints_planner_B.pvt;

          cartesian_waypoints_planner_B.kend =

            cartesian_waypoints_planner_B.ix_g;

          do {

            exitg1 = 0;

            if (cartesian_waypoints_planner_B.kend <=

                cartesian_waypoints_planner_B.ix_g +

                cartesian_waypoints_planner_B.itemp) {

              if (b_A->data[cartesian_waypoints_planner_B.kend - 1] != 0.0) {

                exitg1 = 1;

              } else {

                cartesian_waypoints_planner_B.kend++;

              }

            } else {

              cartesian_waypoints_planner_B.nmi--;

              exitg1 = 2;

            }

          } while (exitg1 == 0);



          if (exitg1 == 1) {

            exitg2 = true;

          }

        }



        cartesian_waypoints_planner_B.lastc = cartesian_waypoints_planner_B.nmi

          - 1;

        cartesian_waypoints_planner_B.kend = c_x->size[0] * c_x->size[1];

        c_x->size[0] = b_A->size[0];

        c_x->size[1] = b_A->size[1];

        cartes_emxEnsureCapacity_real_T(c_x, cartesian_waypoints_planner_B.kend);

        cartesian_waypoints_planner_B.ix_g = b_A->size[0] * b_A->size[1] - 1;

        for (cartesian_waypoints_planner_B.kend = 0;

             cartesian_waypoints_planner_B.kend <=

             cartesian_waypoints_planner_B.ix_g;

             cartesian_waypoints_planner_B.kend++) {

          c_x->data[cartesian_waypoints_planner_B.kend] = b_A->

            data[cartesian_waypoints_planner_B.kend];

        }

      } else {

        cartesian_waypoints_planner_B.itemp = -1;

        cartesian_waypoints_planner_B.lastc = -1;

      }



      if (cartesian_waypoints_planner_B.itemp + 1 > 0) {

        if (cartesian_waypoints_planner_B.lastc + 1 != 0) {

          for (cartesian_waypoints_planner_B.kend = 0;

               cartesian_waypoints_planner_B.kend <=

               cartesian_waypoints_planner_B.lastc;

               cartesian_waypoints_planner_B.kend++) {

            work->data[cartesian_waypoints_planner_B.kend] = 0.0;

          }



          cartesian_waypoints_planner_B.iy_e = 0;

          cartesian_waypoints_planner_B.b_fni = cartesian_waypoints_planner_B.ma

            * cartesian_waypoints_planner_B.lastc +

            cartesian_waypoints_planner_B.pvt;

          for (cartesian_waypoints_planner_B.nmi =

               cartesian_waypoints_planner_B.pvt;

               cartesian_waypoints_planner_B.ma < 0 ?

               cartesian_waypoints_planner_B.nmi >=

               cartesian_waypoints_planner_B.b_fni :

               cartesian_waypoints_planner_B.nmi <=

               cartesian_waypoints_planner_B.b_fni;

               cartesian_waypoints_planner_B.nmi +=

               cartesian_waypoints_planner_B.ma) {

            cartesian_waypoints_planner_B.ix_g =

              cartesian_waypoints_planner_B.ii_c;

            cartesian_waypoints_planner_B.scale_e = 0.0;

            cartesian_waypoints_planner_B.d_n3 =

              cartesian_waypoints_planner_B.nmi +

              cartesian_waypoints_planner_B.itemp;

            for (cartesian_waypoints_planner_B.kend =

                 cartesian_waypoints_planner_B.nmi;

                 cartesian_waypoints_planner_B.kend <=

                 cartesian_waypoints_planner_B.d_n3;

                 cartesian_waypoints_planner_B.kend++) {

              cartesian_waypoints_planner_B.scale_e += c_x->

                data[cartesian_waypoints_planner_B.kend - 1] * c_x->

                data[cartesian_waypoints_planner_B.ix_g];

              cartesian_waypoints_planner_B.ix_g++;

            }



            work->data[cartesian_waypoints_planner_B.iy_e] +=

              cartesian_waypoints_planner_B.scale_e;

            cartesian_waypoints_planner_B.iy_e++;

          }

        }



        if (!(-tau->data[cartesian_waypoints_planner_B.m_n] == 0.0)) {

          cartesian_waypoints_planner_B.iy_e = 0;

          for (cartesian_waypoints_planner_B.kend = 0;

               cartesian_waypoints_planner_B.kend <=

               cartesian_waypoints_planner_B.lastc;

               cartesian_waypoints_planner_B.kend++) {

            if (work->data[cartesian_waypoints_planner_B.iy_e] != 0.0) {

              cartesian_waypoints_planner_B.scale_e = work->

                data[cartesian_waypoints_planner_B.iy_e] * -tau->

                data[cartesian_waypoints_planner_B.m_n];

              cartesian_waypoints_planner_B.ix_g =

                cartesian_waypoints_planner_B.ii_c;

              cartesian_waypoints_planner_B.b_fni =

                cartesian_waypoints_planner_B.itemp +

                cartesian_waypoints_planner_B.pvt;

              for (cartesian_waypoints_planner_B.nmi =

                   cartesian_waypoints_planner_B.pvt;

                   cartesian_waypoints_planner_B.nmi <=

                   cartesian_waypoints_planner_B.b_fni;

                   cartesian_waypoints_planner_B.nmi++) {

                c_x->data[cartesian_waypoints_planner_B.nmi - 1] += c_x->

                  data[cartesian_waypoints_planner_B.ix_g] *

                  cartesian_waypoints_planner_B.scale_e;

                cartesian_waypoints_planner_B.ix_g++;

              }

            }



            cartesian_waypoints_planner_B.iy_e++;

            cartesian_waypoints_planner_B.pvt +=

              cartesian_waypoints_planner_B.ma;

          }

        }

      }



      cartesian_waypoints_planner_B.kend = b_A->size[0] * b_A->size[1];

      b_A->size[0] = c_x->size[0];

      b_A->size[1] = c_x->size[1];

      cartes_emxEnsureCapacity_real_T(b_A, cartesian_waypoints_planner_B.kend);

      cartesian_waypoints_planner_B.ix_g = c_x->size[0] * c_x->size[1] - 1;

      for (cartesian_waypoints_planner_B.kend = 0;

           cartesian_waypoints_planner_B.kend <=

           cartesian_waypoints_planner_B.ix_g;

           cartesian_waypoints_planner_B.kend++) {

        b_A->data[cartesian_waypoints_planner_B.kend] = c_x->

          data[cartesian_waypoints_planner_B.kend];

      }



      b_A->data[cartesian_waypoints_planner_B.ii_c] =

        cartesian_waypoints_planner_B.smax;

    }



    for (cartesian_waypoints_planner_B.ii_c = cartesian_waypoints_planner_B.m_n

         + 2; cartesian_waypoints_planner_B.ii_c <= n;

         cartesian_waypoints_planner_B.ii_c++) {

      cartesian_waypoints_planner_B.pvt = ((cartesian_waypoints_planner_B.ii_c -

        1) * cartesian_waypoints_planner_B.ma +

        cartesian_waypoints_planner_B.m_n) + 1;

      if (vn1->data[cartesian_waypoints_planner_B.ii_c - 1] != 0.0) {

        cartesian_waypoints_planner_B.smax = fabs(b_A->

          data[cartesian_waypoints_planner_B.pvt - 1]) / vn1->

          data[cartesian_waypoints_planner_B.ii_c - 1];

        cartesian_waypoints_planner_B.smax = 1.0 -

          cartesian_waypoints_planner_B.smax *

          cartesian_waypoints_planner_B.smax;

        if (cartesian_waypoints_planner_B.smax < 0.0) {

          cartesian_waypoints_planner_B.smax = 0.0;

        }



        cartesian_waypoints_planner_B.scale_e = vn1->

          data[cartesian_waypoints_planner_B.ii_c - 1] / vn2->

          data[cartesian_waypoints_planner_B.ii_c - 1];

        cartesian_waypoints_planner_B.scale_e =

          cartesian_waypoints_planner_B.scale_e *

          cartesian_waypoints_planner_B.scale_e *

          cartesian_waypoints_planner_B.smax;

        if (cartesian_waypoints_planner_B.scale_e <= 1.4901161193847656E-8) {

          if (cartesian_waypoints_planner_B.m_n + 1 < m) {

            cartesian_waypoints_planner_B.smax = 0.0;

            if (cartesian_waypoints_planner_B.mmi >= 1) {

              if (cartesian_waypoints_planner_B.mmi == 1) {

                cartesian_waypoints_planner_B.smax = fabs(b_A->

                  data[cartesian_waypoints_planner_B.pvt]);

              } else {

                cartesian_waypoints_planner_B.scale_e = 3.3121686421112381E-170;

                cartesian_waypoints_planner_B.kend =

                  cartesian_waypoints_planner_B.pvt +

                  cartesian_waypoints_planner_B.mmi;

                for (cartesian_waypoints_planner_B.itemp =

                     cartesian_waypoints_planner_B.pvt + 1;

                     cartesian_waypoints_planner_B.itemp <=

                     cartesian_waypoints_planner_B.kend;

                     cartesian_waypoints_planner_B.itemp++) {

                  cartesian_waypoints_planner_B.absxk_b = fabs(b_A->

                    data[cartesian_waypoints_planner_B.itemp - 1]);

                  if (cartesian_waypoints_planner_B.absxk_b >

                      cartesian_waypoints_planner_B.scale_e) {

                    cartesian_waypoints_planner_B.t_a =

                      cartesian_waypoints_planner_B.scale_e /

                      cartesian_waypoints_planner_B.absxk_b;

                    cartesian_waypoints_planner_B.smax =

                      cartesian_waypoints_planner_B.smax *

                      cartesian_waypoints_planner_B.t_a *

                      cartesian_waypoints_planner_B.t_a + 1.0;

                    cartesian_waypoints_planner_B.scale_e =

                      cartesian_waypoints_planner_B.absxk_b;

                  } else {

                    cartesian_waypoints_planner_B.t_a =

                      cartesian_waypoints_planner_B.absxk_b /

                      cartesian_waypoints_planner_B.scale_e;

                    cartesian_waypoints_planner_B.smax +=

                      cartesian_waypoints_planner_B.t_a *

                      cartesian_waypoints_planner_B.t_a;

                  }

                }



                cartesian_waypoints_planner_B.smax =

                  cartesian_waypoints_planner_B.scale_e * sqrt

                  (cartesian_waypoints_planner_B.smax);

              }

            }



            vn1->data[cartesian_waypoints_planner_B.ii_c - 1] =

              cartesian_waypoints_planner_B.smax;

            vn2->data[cartesian_waypoints_planner_B.ii_c - 1] = vn1->

              data[cartesian_waypoints_planner_B.ii_c - 1];

          } else {

            vn1->data[cartesian_waypoints_planner_B.ii_c - 1] = 0.0;

            vn2->data[cartesian_waypoints_planner_B.ii_c - 1] = 0.0;

          }

        } else {

          vn1->data[cartesian_waypoints_planner_B.ii_c - 1] *= sqrt

            (cartesian_waypoints_planner_B.smax);

        }

      }

    }

  }



  cartesian_waypoi_emxFree_real_T(&c_x);

  cartesian_waypoi_emxFree_real_T(&vn2);

  cartesian_waypoi_emxFree_real_T(&vn1);

  cartesian_waypoi_emxFree_real_T(&work);

}



static void cartesian_waypoints_pla_xzgetrf(int32_T m, int32_T n, const

  emxArray_real_T_cartesian_way_T *A, int32_T lda,

  emxArray_real_T_cartesian_way_T *b_A, emxArray_int32_T_cartesian_wa_T *ipiv,

  int32_T *info)

{

  int32_T mmj;

  int32_T b;

  int32_T c;

  emxArray_real_T_cartesian_way_T *c_x;

  int32_T ix;

  real_T smax;

  real_T s;

  int32_T iy;

  int32_T n_0;

  int32_T yk;

  int32_T k;

  int32_T jA;

  int32_T jy;

  int32_T c_0;

  int32_T c_tmp;

  k = b_A->size[0] * b_A->size[1];

  b_A->size[0] = A->size[0];

  b_A->size[1] = A->size[1];

  cartes_emxEnsureCapacity_real_T(b_A, k);

  iy = A->size[0] * A->size[1] - 1;

  for (k = 0; k <= iy; k++) {

    b_A->data[k] = A->data[k];

  }



  if (m < n) {

    n_0 = m;

  } else {

    n_0 = n;

  }



  if (n_0 < 1) {

    n_0 = 0;

  }



  k = ipiv->size[0] * ipiv->size[1];

  ipiv->size[0] = 1;

  ipiv->size[1] = n_0;

  carte_emxEnsureCapacity_int32_T(ipiv, k);

  if (n_0 > 0) {

    ipiv->data[0] = 1;

    yk = 1;

    for (k = 2; k <= n_0; k++) {

      yk++;

      ipiv->data[k - 1] = yk;

    }

  }



  yk = 0;

  cartesian_waypoi_emxInit_real_T(&c_x, 2);

  if ((m < 1) || (n < 1)) {

  } else {

    n_0 = m - 1;

    if (n_0 >= n) {

      n_0 = n;

    }



    b = n_0 - 1;

    for (n_0 = 0; n_0 <= b; n_0++) {

      mmj = m - n_0;

      c_tmp = (lda + 1) * n_0;

      c = c_tmp + 2;

      if (mmj < 1) {

        iy = 0;

      } else {

        iy = 1;

        if (mmj > 1) {

          ix = c - 2;

          smax = fabs(b_A->data[c_tmp]);

          for (k = 2; k <= mmj; k++) {

            ix++;

            s = fabs(b_A->data[ix]);

            if (s > smax) {

              iy = k;

              smax = s;

            }

          }

        }

      }



      if (b_A->data[(c + iy) - 3] != 0.0) {

        if (iy - 1 != 0) {

          k = n_0 + iy;

          ipiv->data[n_0] = k;

          ix = c_x->size[0] * c_x->size[1];

          c_x->size[0] = b_A->size[0];

          c_x->size[1] = b_A->size[1];

          cartes_emxEnsureCapacity_real_T(c_x, ix);

          iy = b_A->size[0] * b_A->size[1] - 1;

          for (ix = 0; ix <= iy; ix++) {

            c_x->data[ix] = b_A->data[ix];

          }



          ix = n_0;

          iy = k - 1;

          for (k = 0; k < n; k++) {

            smax = c_x->data[ix];

            c_x->data[ix] = c_x->data[iy];

            c_x->data[iy] = smax;

            ix += lda;

            iy += lda;

          }



          k = b_A->size[0] * b_A->size[1];

          b_A->size[0] = c_x->size[0];

          b_A->size[1] = c_x->size[1];

          cartes_emxEnsureCapacity_real_T(b_A, k);

          iy = c_x->size[0] * c_x->size[1] - 1;

          for (k = 0; k <= iy; k++) {

            b_A->data[k] = c_x->data[k];

          }

        }



        iy = c + mmj;

        for (k = c; k <= iy - 2; k++) {

          b_A->data[k - 1] /= b_A->data[c_tmp];

        }

      } else {

        yk = n_0 + 1;

      }



      iy = (n - n_0) - 2;

      jy = c_tmp + lda;

      jA = jy + 1;

      for (k = 0; k <= iy; k++) {

        smax = b_A->data[jy];

        if (b_A->data[jy] != 0.0) {

          ix = c - 1;

          c_0 = (mmj + jA) - 1;

          for (c_tmp = jA + 1; c_tmp <= c_0; c_tmp++) {

            b_A->data[c_tmp - 1] += b_A->data[ix] * -smax;

            ix++;

          }

        }



        jy += lda;

        jA += lda;

      }

    }



    if ((yk == 0) && (m <= n) && (!(b_A->data[((m - 1) * b_A->size[0] + m) - 1]

          != 0.0))) {

      yk = m;

    }

  }



  cartesian_waypoi_emxFree_real_T(&c_x);

  *info = yk;

}



static void cartesian_waypoints_plann_xtrsm(int32_T m, int32_T n, const

  emxArray_real_T_cartesian_way_T *A, int32_T lda, const

  emxArray_real_T_cartesian_way_T *B, int32_T ldb,

  emxArray_real_T_cartesian_way_T *b_B)

{

  int32_T jBcol;

  int32_T kAcol;

  int32_T i;

  int32_T k;

  int32_T b;

  int32_T b_i;

  int32_T loop_ub;

  int32_T i_0;

  int32_T tmp;

  i_0 = b_B->size[0] * b_B->size[1];

  b_B->size[0] = B->size[0];

  b_B->size[1] = B->size[1];

  cartes_emxEnsureCapacity_real_T(b_B, i_0);

  loop_ub = B->size[0] * B->size[1] - 1;

  for (i_0 = 0; i_0 <= loop_ub; i_0++) {

    b_B->data[i_0] = B->data[i_0];

  }



  if ((n == 0) || ((B->size[0] == 0) || (B->size[1] == 0))) {

  } else {

    for (loop_ub = 0; loop_ub < n; loop_ub++) {

      jBcol = ldb * loop_ub - 1;

      for (k = m; k >= 1; k--) {

        kAcol = (k - 1) * lda - 1;

        i_0 = k + jBcol;

        if (b_B->data[i_0] != 0.0) {

          b_B->data[i_0] /= A->data[k + kAcol];

          b = k - 2;

          for (b_i = 0; b_i <= b; b_i++) {

            i = b_i + 1;

            tmp = i + jBcol;

            b_B->data[tmp] -= b_B->data[i_0] * A->data[i + kAcol];

          }

        }

      }

    }

  }

}



static void cartesian_waypo_emxFree_int32_T(emxArray_int32_T_cartesian_wa_T

  **pEmxArray)

{

  if (*pEmxArray != (emxArray_int32_T_cartesian_wa_T *)NULL) {

    if (((*pEmxArray)->data != (int32_T *)NULL) && (*pEmxArray)->canFreeData) {

      free((*pEmxArray)->data);

    }



    free((*pEmxArray)->size);

    free(*pEmxArray);

    *pEmxArray = (emxArray_int32_T_cartesian_wa_T *)NULL;

  }

}



static void cartesian_waypoints_pl_mldivide(const

  emxArray_real_T_cartesian_way_T *A, const emxArray_real_T_cartesian_way_T *B,

  emxArray_real_T_cartesian_way_T *Y)

{

  emxArray_real_T_cartesian_way_T *c_A;

  emxArray_real_T_cartesian_way_T *b_tau;

  emxArray_int32_T_cartesian_wa_T *b_jpvt;

  emxArray_real_T_cartesian_way_T *B_0;

  emxArray_int32_T_cartesian_wa_T *b_jpvt_0;

  boolean_T guard1 = false;

  cartesian_waypoi_emxInit_real_T(&c_A, 2);

  cartesian_waypoi_emxInit_real_T(&b_tau, 1);

  cartesian_waypo_emxInit_int32_T(&b_jpvt, 2);

  cartesian_waypoi_emxInit_real_T(&B_0, 2);

  cartesian_waypo_emxInit_int32_T(&b_jpvt_0, 2);

  if ((A->size[0] == 0) || (A->size[1] == 0) || ((B->size[0] == 0) || (B->size[1]

        == 0))) {

    cartesian_waypoints_planner_B.minmn = A->size[1];

    cartesian_waypoints_planner_B.minmana = B->size[1];

    cartesian_waypoints_planner_B.b_i_l = Y->size[0] * Y->size[1];

    Y->size[0] = cartesian_waypoints_planner_B.minmn;

    Y->size[1] = cartesian_waypoints_planner_B.minmana;

    cartes_emxEnsureCapacity_real_T(Y, cartesian_waypoints_planner_B.b_i_l);

    cartesian_waypoints_planner_B.minmn = cartesian_waypoints_planner_B.minmn *

      cartesian_waypoints_planner_B.minmana - 1;

    for (cartesian_waypoints_planner_B.b_i_l = 0;

         cartesian_waypoints_planner_B.b_i_l <=

         cartesian_waypoints_planner_B.minmn;

         cartesian_waypoints_planner_B.b_i_l++) {

      Y->data[cartesian_waypoints_planner_B.b_i_l] = 0.0;

    }

  } else if (A->size[0] == A->size[1]) {

    cartesian_waypoints_planner_B.minmn = A->size[0];

    cartesian_waypoints_planner_B.rankR = A->size[1];

    if (cartesian_waypoints_planner_B.minmn <

        cartesian_waypoints_planner_B.rankR) {

      cartesian_waypoints_planner_B.rankR = cartesian_waypoints_planner_B.minmn;

    }



    cartesian_waypoints_planner_B.minmn = B->size[0];

    if (cartesian_waypoints_planner_B.minmn <

        cartesian_waypoints_planner_B.rankR) {

      cartesian_waypoints_planner_B.rankR = cartesian_waypoints_planner_B.minmn;

    }



    cartesian_waypoints_planner_B.nb = B->size[1] - 1;

    cartesian_waypoints_pla_xzgetrf(cartesian_waypoints_planner_B.rankR,

      cartesian_waypoints_planner_B.rankR, A, A->size[0], c_A, b_jpvt,

      &cartesian_waypoints_planner_B.minmn);

    cartesian_waypoints_planner_B.b_i_l = B_0->size[0] * B_0->size[1];

    B_0->size[0] = B->size[0];

    B_0->size[1] = B->size[1];

    cartes_emxEnsureCapacity_real_T(B_0, cartesian_waypoints_planner_B.b_i_l);

    cartesian_waypoints_planner_B.minmn = B->size[0] * B->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_i_l = 0;

         cartesian_waypoints_planner_B.b_i_l <=

         cartesian_waypoints_planner_B.minmn;

         cartesian_waypoints_planner_B.b_i_l++) {

      B_0->data[cartesian_waypoints_planner_B.b_i_l] = B->

        data[cartesian_waypoints_planner_B.b_i_l];

    }



    cartesian_waypoints_planner_B.minmn = cartesian_waypoints_planner_B.rankR -

      2;

    for (cartesian_waypoints_planner_B.b_i_l = 0;

         cartesian_waypoints_planner_B.b_i_l <=

         cartesian_waypoints_planner_B.minmn;

         cartesian_waypoints_planner_B.b_i_l++) {

      if (cartesian_waypoints_planner_B.b_i_l + 1 != b_jpvt->

          data[cartesian_waypoints_planner_B.b_i_l]) {

        cartesian_waypoints_planner_B.na = b_jpvt->

          data[cartesian_waypoints_planner_B.b_i_l] - 1;

        for (cartesian_waypoints_planner_B.minmana = 0;

             cartesian_waypoints_planner_B.minmana <=

             cartesian_waypoints_planner_B.nb;

             cartesian_waypoints_planner_B.minmana++) {

          cartesian_waypoints_planner_B.tol_e = B_0->data[B_0->size[0] *

            cartesian_waypoints_planner_B.minmana +

            cartesian_waypoints_planner_B.b_i_l];

          B_0->data[cartesian_waypoints_planner_B.b_i_l + B_0->size[0] *

            cartesian_waypoints_planner_B.minmana] = B_0->data[B_0->size[0] *

            cartesian_waypoints_planner_B.minmana +

            cartesian_waypoints_planner_B.na];

          B_0->data[cartesian_waypoints_planner_B.na + B_0->size[0] *

            cartesian_waypoints_planner_B.minmana] =

            cartesian_waypoints_planner_B.tol_e;

        }

      }

    }



    if ((B->size[1] == 0) || ((B_0->size[0] == 0) || (B_0->size[1] == 0))) {

    } else {

      for (cartesian_waypoints_planner_B.minmana = 0;

           cartesian_waypoints_planner_B.minmana <=

           cartesian_waypoints_planner_B.nb;

           cartesian_waypoints_planner_B.minmana++) {

        cartesian_waypoints_planner_B.m_p = B->size[0] *

          cartesian_waypoints_planner_B.minmana - 1;

        for (cartesian_waypoints_planner_B.minmn = 0;

             cartesian_waypoints_planner_B.minmn <

             cartesian_waypoints_planner_B.rankR;

             cartesian_waypoints_planner_B.minmn++) {

          cartesian_waypoints_planner_B.nb_l = c_A->size[0] *

            cartesian_waypoints_planner_B.minmn - 1;

          cartesian_waypoints_planner_B.b_i_l =

            (cartesian_waypoints_planner_B.minmn +

             cartesian_waypoints_planner_B.m_p) + 1;

          if (B_0->data[cartesian_waypoints_planner_B.b_i_l] != 0.0) {

            for (cartesian_waypoints_planner_B.na =

                 cartesian_waypoints_planner_B.minmn + 2;

                 cartesian_waypoints_planner_B.na <=

                 cartesian_waypoints_planner_B.rankR;

                 cartesian_waypoints_planner_B.na++) {

              cartesian_waypoints_planner_B.mn =

                cartesian_waypoints_planner_B.na +

                cartesian_waypoints_planner_B.m_p;

              B_0->data[cartesian_waypoints_planner_B.mn] -= B_0->

                data[cartesian_waypoints_planner_B.b_i_l] * c_A->

                data[cartesian_waypoints_planner_B.na +

                cartesian_waypoints_planner_B.nb_l];

            }

          }

        }

      }

    }



    cartesian_waypoints_plann_xtrsm(cartesian_waypoints_planner_B.rankR, B->

      size[1], c_A, c_A->size[0], B_0, B->size[0], Y);

  } else {

    cartesian_waypoints_planner_B.na = A->size[1] - 1;

    cartesian_waypoints_planner_B.b_i_l = c_A->size[0] * c_A->size[1];

    c_A->size[0] = A->size[0];

    c_A->size[1] = A->size[1];

    cartes_emxEnsureCapacity_real_T(c_A, cartesian_waypoints_planner_B.b_i_l);

    cartesian_waypoints_planner_B.minmn = A->size[0] * A->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_i_l = 0;

         cartesian_waypoints_planner_B.b_i_l <=

         cartesian_waypoints_planner_B.minmn;

         cartesian_waypoints_planner_B.b_i_l++) {

      c_A->data[cartesian_waypoints_planner_B.b_i_l] = A->

        data[cartesian_waypoints_planner_B.b_i_l];

    }



    cartesian_waypoints_planner_B.minmn = A->size[0];

    cartesian_waypoints_planner_B.minmana = A->size[1];

    if (cartesian_waypoints_planner_B.minmn <

        cartesian_waypoints_planner_B.minmana) {

      cartesian_waypoints_planner_B.minmana =

        cartesian_waypoints_planner_B.minmn;

    }



    cartesian_waypoints_planner_B.b_i_l = b_tau->size[0];

    b_tau->size[0] = cartesian_waypoints_planner_B.minmana;

    cartes_emxEnsureCapacity_real_T(b_tau, cartesian_waypoints_planner_B.b_i_l);

    for (cartesian_waypoints_planner_B.b_i_l = 0;

         cartesian_waypoints_planner_B.b_i_l <

         cartesian_waypoints_planner_B.minmana;

         cartesian_waypoints_planner_B.b_i_l++) {

      b_tau->data[cartesian_waypoints_planner_B.b_i_l] = 0.0;

    }



    guard1 = false;

    if ((A->size[0] == 0) || (A->size[1] == 0)) {

      guard1 = true;

    } else {

      cartesian_waypoints_planner_B.minmn = A->size[0];

      cartesian_waypoints_planner_B.minmana = A->size[1];

      if (cartesian_waypoints_planner_B.minmn <

          cartesian_waypoints_planner_B.minmana) {

        cartesian_waypoints_planner_B.minmana =

          cartesian_waypoints_planner_B.minmn;

      }



      if (cartesian_waypoints_planner_B.minmana < 1) {

        guard1 = true;

      } else {

        cartesian_waypoints_planner_B.b_i_l = b_jpvt->size[0] * b_jpvt->size[1];

        b_jpvt->size[0] = 1;

        b_jpvt->size[1] = A->size[1];

        carte_emxEnsureCapacity_int32_T(b_jpvt,

          cartesian_waypoints_planner_B.b_i_l);

        cartesian_waypoints_planner_B.minmn = A->size[1] - 1;

        for (cartesian_waypoints_planner_B.b_i_l = 0;

             cartesian_waypoints_planner_B.b_i_l <=

             cartesian_waypoints_planner_B.minmn;

             cartesian_waypoints_planner_B.b_i_l++) {

          b_jpvt->data[cartesian_waypoints_planner_B.b_i_l] = 0;

        }



        for (cartesian_waypoints_planner_B.minmn = 0;

             cartesian_waypoints_planner_B.minmn <=

             cartesian_waypoints_planner_B.na;

             cartesian_waypoints_planner_B.minmn++) {

          b_jpvt->data[cartesian_waypoints_planner_B.minmn] =

            cartesian_waypoints_planner_B.minmn + 1;

        }



        cartesian_waypoints_planner_B.b_i_l = b_jpvt_0->size[0] * b_jpvt_0->

          size[1];

        b_jpvt_0->size[0] = 1;

        b_jpvt_0->size[1] = b_jpvt->size[1];

        carte_emxEnsureCapacity_int32_T(b_jpvt_0,

          cartesian_waypoints_planner_B.b_i_l);

        cartesian_waypoints_planner_B.minmn = b_jpvt->size[0] * b_jpvt->size[1];

        for (cartesian_waypoints_planner_B.b_i_l = 0;

             cartesian_waypoints_planner_B.b_i_l <

             cartesian_waypoints_planner_B.minmn;

             cartesian_waypoints_planner_B.b_i_l++) {

          b_jpvt_0->data[cartesian_waypoints_planner_B.b_i_l] = b_jpvt->

            data[cartesian_waypoints_planner_B.b_i_l];

        }



        cartesian_waypoints_plan_qrpf_e(A, A->size[0], A->size[1], b_tau,

          b_jpvt_0, c_A, b_jpvt);

      }

    }



    if (guard1) {

      cartesian_waypoints_planner_B.b_i_l = b_jpvt->size[0] * b_jpvt->size[1];

      b_jpvt->size[0] = 1;

      b_jpvt->size[1] = A->size[1];

      carte_emxEnsureCapacity_int32_T(b_jpvt,

        cartesian_waypoints_planner_B.b_i_l);

      cartesian_waypoints_planner_B.minmn = A->size[1] - 1;

      for (cartesian_waypoints_planner_B.b_i_l = 0;

           cartesian_waypoints_planner_B.b_i_l <=

           cartesian_waypoints_planner_B.minmn;

           cartesian_waypoints_planner_B.b_i_l++) {

        b_jpvt->data[cartesian_waypoints_planner_B.b_i_l] = 0;

      }



      for (cartesian_waypoints_planner_B.minmana = 0;

           cartesian_waypoints_planner_B.minmana <=

           cartesian_waypoints_planner_B.na;

           cartesian_waypoints_planner_B.minmana++) {

        b_jpvt->data[cartesian_waypoints_planner_B.minmana] =

          cartesian_waypoints_planner_B.minmana + 1;

      }

    }



    cartesian_waypoints_planner_B.rankR = 0;

    if (c_A->size[0] < c_A->size[1]) {

      cartesian_waypoints_planner_B.minmn = c_A->size[0];

      cartesian_waypoints_planner_B.minmana = c_A->size[1];

    } else {

      cartesian_waypoints_planner_B.minmn = c_A->size[1];

      cartesian_waypoints_planner_B.minmana = c_A->size[0];

    }



    if (cartesian_waypoints_planner_B.minmn > 0) {

      cartesian_waypoints_planner_B.tol_e = 2.2204460492503131E-15 *

        static_cast<real_T>(cartesian_waypoints_planner_B.minmana);

      if (1.4901161193847656E-8 < cartesian_waypoints_planner_B.tol_e) {

        cartesian_waypoints_planner_B.tol_e = 1.4901161193847656E-8;

      }



      cartesian_waypoints_planner_B.tol_e *= fabs(c_A->data[0]);

      while ((cartesian_waypoints_planner_B.rankR <

              cartesian_waypoints_planner_B.minmn) && (!(fabs(c_A->data

                [c_A->size[0] * cartesian_waypoints_planner_B.rankR +

                cartesian_waypoints_planner_B.rankR]) <=

               cartesian_waypoints_planner_B.tol_e))) {

        cartesian_waypoints_planner_B.rankR++;

      }

    }



    cartesian_waypoints_planner_B.nb = B->size[1] - 1;

    cartesian_waypoints_planner_B.minmn = c_A->size[1];

    cartesian_waypoints_planner_B.minmana = B->size[1];

    cartesian_waypoints_planner_B.b_i_l = Y->size[0] * Y->size[1];

    Y->size[0] = cartesian_waypoints_planner_B.minmn;

    Y->size[1] = cartesian_waypoints_planner_B.minmana;

    cartes_emxEnsureCapacity_real_T(Y, cartesian_waypoints_planner_B.b_i_l);

    cartesian_waypoints_planner_B.minmn = cartesian_waypoints_planner_B.minmn *

      cartesian_waypoints_planner_B.minmana - 1;

    for (cartesian_waypoints_planner_B.b_i_l = 0;

         cartesian_waypoints_planner_B.b_i_l <=

         cartesian_waypoints_planner_B.minmn;

         cartesian_waypoints_planner_B.b_i_l++) {

      Y->data[cartesian_waypoints_planner_B.b_i_l] = 0.0;

    }



    cartesian_waypoints_planner_B.b_i_l = B_0->size[0] * B_0->size[1];

    B_0->size[0] = B->size[0];

    B_0->size[1] = B->size[1];

    cartes_emxEnsureCapacity_real_T(B_0, cartesian_waypoints_planner_B.b_i_l);

    cartesian_waypoints_planner_B.minmn = B->size[0] * B->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_i_l = 0;

         cartesian_waypoints_planner_B.b_i_l <=

         cartesian_waypoints_planner_B.minmn;

         cartesian_waypoints_planner_B.b_i_l++) {

      B_0->data[cartesian_waypoints_planner_B.b_i_l] = B->

        data[cartesian_waypoints_planner_B.b_i_l];

    }



    cartesian_waypoints_planner_B.m_p = c_A->size[0];

    cartesian_waypoints_planner_B.nb_l = B->size[1] - 1;

    cartesian_waypoints_planner_B.minmn = c_A->size[0];

    cartesian_waypoints_planner_B.minmana = c_A->size[1];

    if (cartesian_waypoints_planner_B.minmn <

        cartesian_waypoints_planner_B.minmana) {

      cartesian_waypoints_planner_B.minmana =

        cartesian_waypoints_planner_B.minmn;

    }



    cartesian_waypoints_planner_B.mn = cartesian_waypoints_planner_B.minmana - 1;

    for (cartesian_waypoints_planner_B.minmana = 0;

         cartesian_waypoints_planner_B.minmana <=

         cartesian_waypoints_planner_B.mn; cartesian_waypoints_planner_B.minmana

         ++) {

      if (b_tau->data[cartesian_waypoints_planner_B.minmana] != 0.0) {

        for (cartesian_waypoints_planner_B.minmn = 0;

             cartesian_waypoints_planner_B.minmn <=

             cartesian_waypoints_planner_B.nb_l;

             cartesian_waypoints_planner_B.minmn++) {

          cartesian_waypoints_planner_B.tol_e = B_0->data[B_0->size[0] *

            cartesian_waypoints_planner_B.minmn +

            cartesian_waypoints_planner_B.minmana];

          for (cartesian_waypoints_planner_B.na =

               cartesian_waypoints_planner_B.minmana + 2;

               cartesian_waypoints_planner_B.na <=

               cartesian_waypoints_planner_B.m_p;

               cartesian_waypoints_planner_B.na++) {

            cartesian_waypoints_planner_B.tol_e += c_A->data[(c_A->size[0] *

              cartesian_waypoints_planner_B.minmana +

              cartesian_waypoints_planner_B.na) - 1] * B_0->data[(B_0->size[0] *

              cartesian_waypoints_planner_B.minmn +

              cartesian_waypoints_planner_B.na) - 1];

          }



          cartesian_waypoints_planner_B.tol_e *= b_tau->

            data[cartesian_waypoints_planner_B.minmana];

          if (cartesian_waypoints_planner_B.tol_e != 0.0) {

            B_0->data[cartesian_waypoints_planner_B.minmana + B_0->size[0] *

              cartesian_waypoints_planner_B.minmn] -=

              cartesian_waypoints_planner_B.tol_e;

            for (cartesian_waypoints_planner_B.b_i_l =

                 cartesian_waypoints_planner_B.minmana + 2;

                 cartesian_waypoints_planner_B.b_i_l <=

                 cartesian_waypoints_planner_B.m_p;

                 cartesian_waypoints_planner_B.b_i_l++) {

              B_0->data[(cartesian_waypoints_planner_B.b_i_l + B_0->size[0] *

                         cartesian_waypoints_planner_B.minmn) - 1] -= c_A->data

                [(c_A->size[0] * cartesian_waypoints_planner_B.minmana +

                  cartesian_waypoints_planner_B.b_i_l) - 1] *

                cartesian_waypoints_planner_B.tol_e;

            }

          }

        }

      }

    }



    for (cartesian_waypoints_planner_B.minmn = 0;

         cartesian_waypoints_planner_B.minmn <= cartesian_waypoints_planner_B.nb;

         cartesian_waypoints_planner_B.minmn++) {

      for (cartesian_waypoints_planner_B.b_i_l = 0;

           cartesian_waypoints_planner_B.b_i_l <

           cartesian_waypoints_planner_B.rankR;

           cartesian_waypoints_planner_B.b_i_l++) {

        Y->data[(b_jpvt->data[cartesian_waypoints_planner_B.b_i_l] + Y->size[0] *

                 cartesian_waypoints_planner_B.minmn) - 1] = B_0->data[B_0->

          size[0] * cartesian_waypoints_planner_B.minmn +

          cartesian_waypoints_planner_B.b_i_l];

      }



      for (cartesian_waypoints_planner_B.minmana =

           cartesian_waypoints_planner_B.rankR;

           cartesian_waypoints_planner_B.minmana >= 1;

           cartesian_waypoints_planner_B.minmana--) {

        Y->data[(b_jpvt->data[cartesian_waypoints_planner_B.minmana - 1] +

                 Y->size[0] * cartesian_waypoints_planner_B.minmn) - 1] /=

          c_A->data[((cartesian_waypoints_planner_B.minmana - 1) * c_A->size[0]

                     + cartesian_waypoints_planner_B.minmana) - 1];

        cartesian_waypoints_planner_B.na = cartesian_waypoints_planner_B.minmana

          - 2;

        for (cartesian_waypoints_planner_B.b_i_l = 0;

             cartesian_waypoints_planner_B.b_i_l <=

             cartesian_waypoints_planner_B.na;

             cartesian_waypoints_planner_B.b_i_l++) {

          Y->data[(b_jpvt->data[cartesian_waypoints_planner_B.b_i_l] + Y->size[0]

                   * cartesian_waypoints_planner_B.minmn) - 1] -= Y->data

            [(b_jpvt->data[cartesian_waypoints_planner_B.minmana - 1] + Y->size

              [0] * cartesian_waypoints_planner_B.minmn) - 1] * c_A->data

            [(cartesian_waypoints_planner_B.minmana - 1) * c_A->size[0] +

            cartesian_waypoints_planner_B.b_i_l];

        }

      }

    }

  }



  cartesian_waypo_emxFree_int32_T(&b_jpvt_0);

  cartesian_waypoi_emxFree_real_T(&B_0);

  cartesian_waypo_emxFree_int32_T(&b_jpvt);

  cartesian_waypoi_emxFree_real_T(&b_tau);

  cartesian_waypoi_emxFree_real_T(&c_A);

}



static void cartesian_way_emxFree_boolean_T(emxArray_boolean_T_cartesian__T

  **pEmxArray)

{

  if (*pEmxArray != (emxArray_boolean_T_cartesian__T *)NULL) {

    if (((*pEmxArray)->data != (boolean_T *)NULL) && (*pEmxArray)->canFreeData)

    {

      free((*pEmxArray)->data);

    }



    free((*pEmxArray)->size);

    free(*pEmxArray);

    *pEmxArray = (emxArray_boolean_T_cartesian__T *)NULL;

  }

}



static boolean_T DampedBFGSwGradientProjection_a(const

  h_robotics_core_internal_Damp_T *obj, const real_T Hg[6], const

  emxArray_real_T_cartesian_way_T *alpha)

{

  boolean_T flag;

  boolean_T y;

  emxArray_boolean_T_cartesian__T *x;

  int32_T ix;

  int32_T loop_ub;

  boolean_T exitg1;

  cartesian_way_emxInit_boolean_T(&x, 1);

  if (cartesian_waypoints_plan_norm_e(Hg) < obj->GradientTolerance) {

    ix = x->size[0];

    x->size[0] = alpha->size[0];

    car_emxEnsureCapacity_boolean_T(x, ix);

    loop_ub = alpha->size[0];

    for (ix = 0; ix < loop_ub; ix++) {

      x->data[ix] = (alpha->data[ix] <= 0.0);

    }



    y = true;

    ix = 0;

    exitg1 = false;

    while ((!exitg1) && (ix + 1 <= x->size[0])) {

      if (!x->data[ix]) {

        y = false;

        exitg1 = true;

      } else {

        ix++;

      }

    }



    if (y) {

      flag = true;

    } else {

      flag = false;

    }

  } else {

    flag = false;

  }



  cartesian_way_emxFree_boolean_T(&x);

  return flag;

}



static void cartesian_waypoints_planner_inv(const

  emxArray_real_T_cartesian_way_T *x, emxArray_real_T_cartesian_way_T *y)

{

  int32_T n;

  emxArray_int32_T_cartesian_wa_T *p;

  int32_T c;

  emxArray_real_T_cartesian_way_T *c_A;

  emxArray_int32_T_cartesian_wa_T *b_ipiv;

  int32_T info;

  int32_T n_0;

  int32_T yk;

  emxArray_real_T_cartesian_way_T *y_0;

  if ((x->size[0] == 0) || (x->size[1] == 0)) {

    info = y->size[0] * y->size[1];

    y->size[0] = x->size[0];

    y->size[1] = x->size[1];

    cartes_emxEnsureCapacity_real_T(y, info);

    n_0 = x->size[0] * x->size[1] - 1;

    for (info = 0; info <= n_0; info++) {

      y->data[info] = x->data[info];

    }

  } else {

    n = x->size[0];

    info = y->size[0] * y->size[1];

    y->size[0] = x->size[0];

    y->size[1] = x->size[1];

    cartes_emxEnsureCapacity_real_T(y, info);

    n_0 = x->size[0] * x->size[1] - 1;

    for (info = 0; info <= n_0; info++) {

      y->data[info] = 0.0;

    }



    cartesian_waypo_emxInit_int32_T(&p, 2);

    cartesian_waypoi_emxInit_real_T(&c_A, 2);

    cartesian_waypo_emxInit_int32_T(&b_ipiv, 2);

    cartesian_waypoints_pla_xzgetrf(x->size[0], x->size[0], x, x->size[0], c_A,

      b_ipiv, &info);

    if (x->size[0] < 1) {

      n_0 = 0;

    } else {

      n_0 = x->size[0];

    }



    info = p->size[0] * p->size[1];

    p->size[0] = 1;

    p->size[1] = n_0;

    carte_emxEnsureCapacity_int32_T(p, info);

    if (n_0 > 0) {

      p->data[0] = 1;

      yk = 1;

      for (info = 2; info <= n_0; info++) {

        yk++;

        p->data[info - 1] = yk;

      }

    }



    n_0 = b_ipiv->size[1] - 1;

    for (info = 0; info <= n_0; info++) {

      if (b_ipiv->data[info] > static_cast<real_T>(info) + 1.0) {

        yk = p->data[b_ipiv->data[info] - 1];

        p->data[b_ipiv->data[info] - 1] = p->data[info];

        p->data[info] = yk;

      }

    }



    cartesian_waypo_emxFree_int32_T(&b_ipiv);

    for (info = 0; info < n; info++) {

      c = p->data[info] - 1;

      y->data[info + y->size[0] * (p->data[info] - 1)] = 1.0;

      for (n_0 = info + 1; n_0 <= n; n_0++) {

        if (y->data[(y->size[0] * c + n_0) - 1] != 0.0) {

          for (yk = n_0 + 1; yk <= n; yk++) {

            y->data[(yk + y->size[0] * c) - 1] -= c_A->data[((n_0 - 1) *

              c_A->size[0] + yk) - 1] * y->data[(y->size[0] * c + n_0) - 1];

          }

        }

      }

    }



    cartesian_waypo_emxFree_int32_T(&p);

    cartesian_waypoi_emxInit_real_T(&y_0, 2);

    info = y_0->size[0] * y_0->size[1];

    y_0->size[0] = y->size[0];

    y_0->size[1] = y->size[1];

    cartes_emxEnsureCapacity_real_T(y_0, info);

    n_0 = y->size[0] * y->size[1];

    for (info = 0; info < n_0; info++) {

      y_0->data[info] = y->data[info];

    }



    cartesian_waypoints_plann_xtrsm(x->size[0], x->size[0], c_A, x->size[0], y_0,

      x->size[0], y);

    cartesian_waypoi_emxFree_real_T(&y_0);

    cartesian_waypoi_emxFree_real_T(&c_A);

  }

}



static void cartesian_waypoints_planne_diag(const

  emxArray_real_T_cartesian_way_T *v, emxArray_real_T_cartesian_way_T *d)

{

  int32_T u0;

  int32_T u1;

  if ((v->size[0] == 1) && (v->size[1] == 1)) {

    u0 = d->size[0];

    d->size[0] = 1;

    cartes_emxEnsureCapacity_real_T(d, u0);

    d->data[0] = v->data[0];

  } else {

    if (0 < v->size[1]) {

      u0 = v->size[0];

      u1 = v->size[1];

      if (u0 < u1) {

        u1 = u0;

      }

    } else {

      u1 = 0;

    }



    u0 = d->size[0];

    d->size[0] = u1;

    cartes_emxEnsureCapacity_real_T(d, u0);

    for (u0 = 0; u0 < u1; u0++) {

      d->data[u0] = v->data[v->size[0] * u0 + u0];

    }

  }

}



static void cartesian_waypoints_pla_sqrt_ev(emxArray_real_T_cartesian_way_T *x)

{

  int32_T nx;

  int32_T b_k;

  nx = x->size[0] - 1;

  for (b_k = 0; b_k <= nx; b_k++) {

    x->data[b_k] = sqrt(x->data[b_k]);

  }

}



static boolean_T cartesian_waypoints_planner_any(const

  emxArray_boolean_T_cartesian__T *x)

{

  boolean_T y;

  int32_T ix;

  boolean_T exitg1;

  y = false;

  ix = 0;

  exitg1 = false;

  while ((!exitg1) && (ix + 1 <= x->size[0])) {

    if (!x->data[ix]) {

      ix++;

    } else {

      y = true;

      exitg1 = true;

    }

  }



  return y;

}



static boolean_T cartesian_wa_isPositiveDefinite(const real_T B[36])

{

  emxArray_real_T_cartesian_way_T *b_x;

  boolean_T exitg1;

  cartesian_waypoints_planner_B.c_A_size_idx_0 = 6;

  cartesian_waypoints_planner_B.c_A_size_idx_1 = 6;

  memcpy(&cartesian_waypoints_planner_B.c_A_data[0], &B[0], 36U * sizeof(real_T));

  cartesian_waypoints_planner_B.b_info = 0;

  cartesian_waypoints_planner_B.b_j_j = 1;

  cartesian_waypoi_emxInit_real_T(&b_x, 2);

  exitg1 = false;

  while ((!exitg1) && (cartesian_waypoints_planner_B.b_j_j - 1 < 6)) {

    cartesian_waypoints_planner_B.jm1 = cartesian_waypoints_planner_B.b_j_j - 2;

    cartesian_waypoints_planner_B.idxAjj = ((cartesian_waypoints_planner_B.b_j_j

      - 1) * 6 + cartesian_waypoints_planner_B.b_j_j) - 1;

    cartesian_waypoints_planner_B.ssq = 0.0;

    if (cartesian_waypoints_planner_B.b_j_j - 1 >= 1) {

      cartesian_waypoints_planner_B.ix_j = cartesian_waypoints_planner_B.b_j_j -

        1;

      cartesian_waypoints_planner_B.iy = cartesian_waypoints_planner_B.b_j_j - 1;

      for (cartesian_waypoints_planner_B.k_l = 0;

           cartesian_waypoints_planner_B.k_l <=

           cartesian_waypoints_planner_B.jm1; cartesian_waypoints_planner_B.k_l

           ++) {

        cartesian_waypoints_planner_B.ssq +=

          cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.ix_j]

          * cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.iy];

        cartesian_waypoints_planner_B.ix_j += 6;

        cartesian_waypoints_planner_B.iy += 6;

      }

    }



    cartesian_waypoints_planner_B.ssq =

      cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.idxAjj]

      - cartesian_waypoints_planner_B.ssq;

    if (cartesian_waypoints_planner_B.ssq > 0.0) {

      cartesian_waypoints_planner_B.ssq = sqrt(cartesian_waypoints_planner_B.ssq);

      cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.idxAjj]

        = cartesian_waypoints_planner_B.ssq;

      if (cartesian_waypoints_planner_B.b_j_j < 6) {

        if (cartesian_waypoints_planner_B.b_j_j - 1 != 0) {

          cartesian_waypoints_planner_B.ix_j =

            cartesian_waypoints_planner_B.b_j_j - 1;

          cartesian_waypoints_planner_B.jm1 =

            (cartesian_waypoints_planner_B.b_j_j - 2) * 6 +

            cartesian_waypoints_planner_B.b_j_j;

          for (cartesian_waypoints_planner_B.k_l =

               cartesian_waypoints_planner_B.b_j_j + 1;

               cartesian_waypoints_planner_B.k_l <=

               cartesian_waypoints_planner_B.jm1 + 1;

               cartesian_waypoints_planner_B.k_l += 6) {

            cartesian_waypoints_planner_B.c =

              -cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.ix_j];

            cartesian_waypoints_planner_B.iy =

              cartesian_waypoints_planner_B.idxAjj + 1;

            cartesian_waypoints_planner_B.d_np =

              cartesian_waypoints_planner_B.k_l -

              cartesian_waypoints_planner_B.b_j_j;

            for (cartesian_waypoints_planner_B.ia =

                 cartesian_waypoints_planner_B.k_l;

                 cartesian_waypoints_planner_B.ia <=

                 cartesian_waypoints_planner_B.d_np + 5;

                 cartesian_waypoints_planner_B.ia++) {

              cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.iy]

                +=

                cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.ia

                - 1] * cartesian_waypoints_planner_B.c;

              cartesian_waypoints_planner_B.iy++;

            }



            cartesian_waypoints_planner_B.ix_j += 6;

          }

        }



        cartesian_waypoints_planner_B.ssq = 1.0 /

          cartesian_waypoints_planner_B.ssq;

        cartesian_waypoints_planner_B.ix_j = b_x->size[0] * b_x->size[1];

        b_x->size[0] = 6;

        b_x->size[1] = 6;

        cartes_emxEnsureCapacity_real_T(b_x, cartesian_waypoints_planner_B.ix_j);

        cartesian_waypoints_planner_B.c_A_size_idx_0 =

          cartesian_waypoints_planner_B.c_A_size_idx_0 *

          cartesian_waypoints_planner_B.c_A_size_idx_1 - 1;

        for (cartesian_waypoints_planner_B.ix_j = 0;

             cartesian_waypoints_planner_B.ix_j <=

             cartesian_waypoints_planner_B.c_A_size_idx_0;

             cartesian_waypoints_planner_B.ix_j++) {

          b_x->data[cartesian_waypoints_planner_B.ix_j] =

            cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.ix_j];

        }



        cartesian_waypoints_planner_B.jm1 = cartesian_waypoints_planner_B.idxAjj

          - cartesian_waypoints_planner_B.b_j_j;

        for (cartesian_waypoints_planner_B.k_l =

             cartesian_waypoints_planner_B.idxAjj + 2;

             cartesian_waypoints_planner_B.k_l <=

             cartesian_waypoints_planner_B.jm1 + 7;

             cartesian_waypoints_planner_B.k_l++) {

          b_x->data[cartesian_waypoints_planner_B.k_l - 1] *=

            cartesian_waypoints_planner_B.ssq;

        }



        cartesian_waypoints_planner_B.c_A_size_idx_0 = b_x->size[0];

        cartesian_waypoints_planner_B.c_A_size_idx_1 = b_x->size[1];

        for (cartesian_waypoints_planner_B.ix_j = 0;

             cartesian_waypoints_planner_B.ix_j < 36;

             cartesian_waypoints_planner_B.ix_j++) {

          cartesian_waypoints_planner_B.c_A_data[cartesian_waypoints_planner_B.ix_j]

            = b_x->data[cartesian_waypoints_planner_B.ix_j];

        }

      }



      cartesian_waypoints_planner_B.b_j_j++;

    } else {

      cartesian_waypoints_planner_B.b_info = cartesian_waypoints_planner_B.b_j_j;

      exitg1 = true;

    }

  }



  cartesian_waypoi_emxFree_real_T(&b_x);

  return cartesian_waypoints_planner_B.b_info == 0;

}



static boolean_T DampedBFGSwGradientProjection_e(const

  h_robotics_core_internal_Damp_T *obj, const real_T xNew[6])

{

  boolean_T flag;

  emxArray_real_T_cartesian_way_T *b;

  emxArray_real_T_cartesian_way_T *c;

  int32_T m;

  int32_T inner;

  int32_T b_i;

  int32_T loop_ub;

  emxArray_boolean_T_cartesian__T *c_0;

  cartesian_waypoi_emxInit_real_T(&b, 2);

  cartesian_waypoi_emxInit_real_T(&c, 1);

  cartesian_way_emxInit_boolean_T(&c_0, 1);

  if (obj->ConstraintsOn) {

    b_i = b->size[0] * b->size[1];

    b->size[0] = obj->ConstraintMatrix->size[0];

    b->size[1] = obj->ConstraintMatrix->size[1];

    cartes_emxEnsureCapacity_real_T(b, b_i);

    loop_ub = obj->ConstraintMatrix->size[0] * obj->ConstraintMatrix->size[1] -

      1;

    for (b_i = 0; b_i <= loop_ub; b_i++) {

      b->data[b_i] = obj->ConstraintMatrix->data[b_i];

    }



    m = b->size[1] - 1;

    inner = b->size[0] - 1;

    b_i = c->size[0];

    c->size[0] = b->size[1];

    cartes_emxEnsureCapacity_real_T(c, b_i);

    for (b_i = 0; b_i <= m; b_i++) {

      c->data[b_i] = 0.0;

    }



    for (b_i = 0; b_i <= inner; b_i++) {

      for (loop_ub = 0; loop_ub <= m; loop_ub++) {

        c->data[loop_ub] += b->data[loop_ub * b->size[0] + b_i] * xNew[b_i];

      }

    }



    b_i = c_0->size[0];

    c_0->size[0] = c->size[0];

    car_emxEnsureCapacity_boolean_T(c_0, b_i);

    loop_ub = c->size[0];

    for (b_i = 0; b_i < loop_ub; b_i++) {

      c_0->data[b_i] = (c->data[b_i] - obj->ConstraintBound->data[b_i] >

                        1.4901161193847656E-8);

    }



    if (cartesian_waypoints_planner_any(c_0)) {

      flag = true;

    } else {

      flag = false;

    }

  } else {

    flag = false;

  }



  cartesian_way_emxFree_boolean_T(&c_0);

  cartesian_waypoi_emxFree_real_T(&c);

  cartesian_waypoi_emxFree_real_T(&b);

  return flag;

}



static void DampedBFGSwGradientProjection_s(h_robotics_core_internal_Damp_T *obj,

  real_T xSol[6], c_robotics_core_internal_NLPS_T *exitFlag, real_T *err, real_T

  *iter)

{

  emxArray_real_T_cartesian_way_T *unusedU1;

  emxArray_real_T_cartesian_way_T *grad;

  emxArray_boolean_T_cartesian__T *activeSet;

  emxArray_real_T_cartesian_way_T *A;

  emxArray_real_T_cartesian_way_T *alpha;

  emxArray_real_T_cartesian_way_T *AIn;

  emxArray_real_T_cartesian_way_T *L;

  f_robotics_manip_internal_IKE_T *b;

  f_robotics_manip_internal_IKE_T *c;

  f_robotics_manip_internal_IKE_T *d;

  emxArray_int32_T_cartesian_wa_T *cb;

  emxArray_int32_T_cartesian_wa_T *db;

  emxArray_int32_T_cartesian_wa_T *eb;

  emxArray_int32_T_cartesian_wa_T *fb;

  emxArray_int32_T_cartesian_wa_T *gb;

  f_robotics_manip_internal_IKE_T *args;

  emxArray_real_T_cartesian_way_T *a;

  emxArray_int32_T_cartesian_wa_T *ii;

  emxArray_real_T_cartesian_way_T *y;

  emxArray_int32_T_cartesian_wa_T *ii_0;

  emxArray_real_T_cartesian_way_T *y_0;

  emxArray_boolean_T_cartesian__T *x;

  emxArray_real_T_cartesian_way_T *A_0;

  emxArray_real_T_cartesian_way_T *A_1;

  emxArray_real_T_cartesian_way_T *A_2;

  emxArray_real_T_cartesian_way_T *sigma;

  emxArray_real_T_cartesian_way_T *tmp;

  emxArray_real_T_cartesian_way_T *tmp_0;

  emxArray_real_T_cartesian_way_T *grad_0;

  emxArray_real_T_cartesian_way_T *sNew;

  emxArray_int32_T_cartesian_wa_T *ii_1;

  emxArray_int32_T_cartesian_wa_T *ii_2;

  emxArray_real_T_cartesian_way_T *grad_1;

  emxArray_real_T_cartesian_way_T *A_3;

  emxArray_real_T_cartesian_way_T *alpha_0;

  emxArray_int32_T_cartesian_wa_T *ii_3;

  static const int8_T tmp_1[36] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1,

    0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1 };



  int32_T exitg1;

  int32_T exitg2;

  boolean_T exitg3;

  boolean_T guard1 = false;

  boolean_T guard2 = false;

  for (cartesian_waypoints_planner_B.i_h = 0; cartesian_waypoints_planner_B.i_h <

       6; cartesian_waypoints_planner_B.i_h++) {

    cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.i_h] =

      obj->SeedInternal[cartesian_waypoints_planner_B.i_h];

  }



  cartesian_waypoi_emxInit_real_T(&unusedU1, 2);

  cartesian_waypoi_emxInit_real_T(&grad, 1);

  obj->TimeObjInternal.StartTime = ctimefun();

  cartesian_IKHelpers_computeCost(cartesian_waypoints_planner_B.x,

    obj->ExtraArgs, &cartesian_waypoints_planner_B.cost,

    cartesian_waypoints_planner_B.unusedU0, unusedU1, &b);

  obj->ExtraArgs = b;

  args = obj->ExtraArgs;

  cartesian_waypoints_planner_B.b_i_h = grad->size[0];

  grad->size[0] = args->GradTemp->size[0];

  cartes_emxEnsureCapacity_real_T(grad, cartesian_waypoints_planner_B.b_i_h);

  cartesian_waypoints_planner_B.i_h = args->GradTemp->size[0];

  for (cartesian_waypoints_planner_B.b_i_h = 0;

       cartesian_waypoints_planner_B.b_i_h < cartesian_waypoints_planner_B.i_h;

       cartesian_waypoints_planner_B.b_i_h++) {

    grad->data[cartesian_waypoints_planner_B.b_i_h] = args->GradTemp->

      data[cartesian_waypoints_planner_B.b_i_h];

  }



  cartesian_waypoints_planner_eye(cartesian_waypoints_planner_B.unusedU0);

  memcpy(&cartesian_waypoints_planner_B.H[0],

         &cartesian_waypoints_planner_B.unusedU0[0], 36U * sizeof(real_T));

  cartesian_way_emxInit_boolean_T(&activeSet, 1);

  cartesian_waypoi_emxInit_real_T(&A, 2);

  cartesian_waypoi_emxInit_real_T(&alpha, 1);

  cartesian_waypo_emxInit_int32_T(&ii, 1);

  if (obj->ConstraintsOn) {

    cartesian_waypoints_planner_B.b_i_h = A->size[0] * A->size[1];

    A->size[0] = obj->ConstraintMatrix->size[0];

    A->size[1] = obj->ConstraintMatrix->size[1];

    cartes_emxEnsureCapacity_real_T(A, cartesian_waypoints_planner_B.b_i_h);

    cartesian_waypoints_planner_B.i_h = obj->ConstraintMatrix->size[0] *

      obj->ConstraintMatrix->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_i_h = 0;

         cartesian_waypoints_planner_B.b_i_h <=

         cartesian_waypoints_planner_B.i_h; cartesian_waypoints_planner_B.b_i_h

         ++) {

      A->data[cartesian_waypoints_planner_B.b_i_h] = obj->ConstraintMatrix->

        data[cartesian_waypoints_planner_B.b_i_h];

    }



    cartesian_waypoints_planner_B.m_ew = A->size[1] - 1;

    cartesian_waypoints_planner_B.inner = A->size[0] - 1;

    cartesian_waypoints_planner_B.b_i_h = alpha->size[0];

    alpha->size[0] = A->size[1];

    cartes_emxEnsureCapacity_real_T(alpha, cartesian_waypoints_planner_B.b_i_h);

    for (cartesian_waypoints_planner_B.b_i_h = 0;

         cartesian_waypoints_planner_B.b_i_h <=

         cartesian_waypoints_planner_B.m_ew; cartesian_waypoints_planner_B.b_i_h

         ++) {

      alpha->data[cartesian_waypoints_planner_B.b_i_h] = 0.0;

    }



    for (cartesian_waypoints_planner_B.nx_f = 0;

         cartesian_waypoints_planner_B.nx_f <=

         cartesian_waypoints_planner_B.inner; cartesian_waypoints_planner_B.nx_f

         ++) {

      for (cartesian_waypoints_planner_B.g_idx_0 = 0;

           cartesian_waypoints_planner_B.g_idx_0 <=

           cartesian_waypoints_planner_B.m_ew;

           cartesian_waypoints_planner_B.g_idx_0++) {

        alpha->data[cartesian_waypoints_planner_B.g_idx_0] += A->

          data[cartesian_waypoints_planner_B.g_idx_0 * A->size[0] +

          cartesian_waypoints_planner_B.nx_f] *

          cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.nx_f];

      }

    }



    cartesian_waypoints_planner_B.b_i_h = activeSet->size[0];

    activeSet->size[0] = alpha->size[0];

    car_emxEnsureCapacity_boolean_T(activeSet,

      cartesian_waypoints_planner_B.b_i_h);

    cartesian_waypoints_planner_B.i_h = alpha->size[0];

    for (cartesian_waypoints_planner_B.b_i_h = 0;

         cartesian_waypoints_planner_B.b_i_h < cartesian_waypoints_planner_B.i_h;

         cartesian_waypoints_planner_B.b_i_h++) {

      activeSet->data[cartesian_waypoints_planner_B.b_i_h] = (alpha->

        data[cartesian_waypoints_planner_B.b_i_h] >= obj->ConstraintBound->

        data[cartesian_waypoints_planner_B.b_i_h]);

    }



    cartesian_waypoints_planner_B.nx_f = activeSet->size[0] - 1;

    cartesian_waypoints_planner_B.idx = 0;

    for (cartesian_waypoints_planner_B.g_idx_0 = 0;

         cartesian_waypoints_planner_B.g_idx_0 <=

         cartesian_waypoints_planner_B.nx_f;

         cartesian_waypoints_planner_B.g_idx_0++) {

      if (activeSet->data[cartesian_waypoints_planner_B.g_idx_0]) {

        cartesian_waypoints_planner_B.idx++;

      }

    }



    cartesian_waypoints_planner_B.b_i_h = ii->size[0];

    ii->size[0] = cartesian_waypoints_planner_B.idx;

    carte_emxEnsureCapacity_int32_T(ii, cartesian_waypoints_planner_B.b_i_h);

    cartesian_waypoints_planner_B.b_i_h = 0;

    for (cartesian_waypoints_planner_B.g_idx_0 = 0;

         cartesian_waypoints_planner_B.g_idx_0 <=

         cartesian_waypoints_planner_B.nx_f;

         cartesian_waypoints_planner_B.g_idx_0++) {

      if (activeSet->data[cartesian_waypoints_planner_B.g_idx_0]) {

        ii->data[cartesian_waypoints_planner_B.b_i_h] =

          cartesian_waypoints_planner_B.g_idx_0 + 1;

        cartesian_waypoints_planner_B.b_i_h++;

      }

    }



    cartesian_waypoints_planner_B.i_h = obj->ConstraintMatrix->size[0];

    cartesian_waypoints_planner_B.b_i_h = A->size[0] * A->size[1];

    A->size[0] = cartesian_waypoints_planner_B.i_h;

    A->size[1] = ii->size[0];

    cartes_emxEnsureCapacity_real_T(A, cartesian_waypoints_planner_B.b_i_h);

    cartesian_waypoints_planner_B.n_n = ii->size[0];

    for (cartesian_waypoints_planner_B.b_i_h = 0;

         cartesian_waypoints_planner_B.b_i_h < cartesian_waypoints_planner_B.n_n;

         cartesian_waypoints_planner_B.b_i_h++) {

      for (cartesian_waypoints_planner_B.idx = 0;

           cartesian_waypoints_planner_B.idx < cartesian_waypoints_planner_B.i_h;

           cartesian_waypoints_planner_B.idx++) {

        A->data[cartesian_waypoints_planner_B.idx + A->size[0] *

          cartesian_waypoints_planner_B.b_i_h] = obj->ConstraintMatrix->data

          [(ii->data[cartesian_waypoints_planner_B.b_i_h] - 1) *

          obj->ConstraintMatrix->size[0] + cartesian_waypoints_planner_B.idx];

      }

    }

  } else {

    cartesian_waypoints_planner_B.g_idx_0 = obj->ConstraintBound->size[0];

    cartesian_waypoints_planner_B.b_i_h = activeSet->size[0];

    activeSet->size[0] = cartesian_waypoints_planner_B.g_idx_0;

    car_emxEnsureCapacity_boolean_T(activeSet,

      cartesian_waypoints_planner_B.b_i_h);

    for (cartesian_waypoints_planner_B.b_i_h = 0;

         cartesian_waypoints_planner_B.b_i_h <

         cartesian_waypoints_planner_B.g_idx_0;

         cartesian_waypoints_planner_B.b_i_h++) {

      activeSet->data[cartesian_waypoints_planner_B.b_i_h] = false;

    }



    A->size[0] = 6;

    A->size[1] = 0;

  }



  cartesian_waypoints_planner_B.j_p = A->size[1] - 1;

  cartesian_waypoi_emxInit_real_T(&AIn, 2);

  cartesian_waypoi_emxInit_real_T(&A_0, 2);

  cartesian_waypoi_emxInit_real_T(&A_1, 1);

  for (cartesian_waypoints_planner_B.nx_f = 0;

       cartesian_waypoints_planner_B.nx_f <= cartesian_waypoints_planner_B.j_p;

       cartesian_waypoints_planner_B.nx_f++) {

    cartesian_waypoints_planner_B.i_h = A->size[0];

    cartesian_waypoints_planner_B.b_i_h = A_0->size[0] * A_0->size[1];

    A_0->size[0] = 1;

    A_0->size[1] = cartesian_waypoints_planner_B.i_h;

    cartes_emxEnsureCapacity_real_T(A_0, cartesian_waypoints_planner_B.b_i_h);

    for (cartesian_waypoints_planner_B.b_i_h = 0;

         cartesian_waypoints_planner_B.b_i_h < cartesian_waypoints_planner_B.i_h;

         cartesian_waypoints_planner_B.b_i_h++) {

      A_0->data[cartesian_waypoints_planner_B.b_i_h] = A->data[A->size[0] *

        cartesian_waypoints_planner_B.nx_f + cartesian_waypoints_planner_B.b_i_h];

    }



    cartesian_waypoints_planner_B.i_h = A->size[0];

    cartesian_waypoints_planner_B.b_i_h = A_1->size[0];

    A_1->size[0] = cartesian_waypoints_planner_B.i_h;

    cartes_emxEnsureCapacity_real_T(A_1, cartesian_waypoints_planner_B.b_i_h);

    for (cartesian_waypoints_planner_B.b_i_h = 0;

         cartesian_waypoints_planner_B.b_i_h < cartesian_waypoints_planner_B.i_h;

         cartesian_waypoints_planner_B.b_i_h++) {

      A_1->data[cartesian_waypoints_planner_B.b_i_h] = A->data[A->size[0] *

        cartesian_waypoints_planner_B.nx_f + cartesian_waypoints_planner_B.b_i_h];

    }



    cartesian_waypoints_planner_B.A_d = 0.0;

    for (cartesian_waypoints_planner_B.b_i_h = 0;

         cartesian_waypoints_planner_B.b_i_h < 6;

         cartesian_waypoints_planner_B.b_i_h++) {

      cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h] =

        0.0;

      for (cartesian_waypoints_planner_B.idx = 0;

           cartesian_waypoints_planner_B.idx < 6;

           cartesian_waypoints_planner_B.idx++) {

        cartesian_waypoints_planner_B.s_i = cartesian_waypoints_planner_B.H[6 *

          cartesian_waypoints_planner_B.b_i_h +

          cartesian_waypoints_planner_B.idx] * A_0->

          data[cartesian_waypoints_planner_B.idx] +

          cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h];

        cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h] =

          cartesian_waypoints_planner_B.s_i;

      }



      cartesian_waypoints_planner_B.A_d +=

        cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h] *

        A_1->data[cartesian_waypoints_planner_B.b_i_h];

    }



    cartesian_waypoints_planner_B.s_i = 1.0 / cartesian_waypoints_planner_B.A_d;

    for (cartesian_waypoints_planner_B.b_i_h = 0;

         cartesian_waypoints_planner_B.b_i_h < 36;

         cartesian_waypoints_planner_B.b_i_h++) {

      cartesian_waypoints_planner_B.V[cartesian_waypoints_planner_B.b_i_h] =

        cartesian_waypoints_planner_B.s_i *

        cartesian_waypoints_planner_B.H[cartesian_waypoints_planner_B.b_i_h];

    }



    cartesian_waypoints_planner_B.i_h = A->size[0];

    cartesian_waypoints_planner_B.n_n = A->size[0];

    cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];

    AIn->size[0] = cartesian_waypoints_planner_B.i_h;

    AIn->size[1] = cartesian_waypoints_planner_B.n_n;

    cartes_emxEnsureCapacity_real_T(AIn, cartesian_waypoints_planner_B.b_i_h);

    for (cartesian_waypoints_planner_B.b_i_h = 0;

         cartesian_waypoints_planner_B.b_i_h < cartesian_waypoints_planner_B.n_n;

         cartesian_waypoints_planner_B.b_i_h++) {

      for (cartesian_waypoints_planner_B.idx = 0;

           cartesian_waypoints_planner_B.idx < cartesian_waypoints_planner_B.i_h;

           cartesian_waypoints_planner_B.idx++) {

        AIn->data[cartesian_waypoints_planner_B.idx + AIn->size[0] *

          cartesian_waypoints_planner_B.b_i_h] = A->data[A->size[0] *

          cartesian_waypoints_planner_B.nx_f + cartesian_waypoints_planner_B.idx]

          * A->data[A->size[0] * cartesian_waypoints_planner_B.nx_f +

          cartesian_waypoints_planner_B.b_i_h];

      }

    }



    cartesian_waypoints_planner_B.n_n = AIn->size[1] - 1;

    cartesian_waypoints_planner_B.b_i_h = unusedU1->size[0] * unusedU1->size[1];

    unusedU1->size[0] = 6;

    unusedU1->size[1] = AIn->size[1];

    cartes_emxEnsureCapacity_real_T(unusedU1,

      cartesian_waypoints_planner_B.b_i_h);

    for (cartesian_waypoints_planner_B.idx = 0;

         cartesian_waypoints_planner_B.idx <= cartesian_waypoints_planner_B.n_n;

         cartesian_waypoints_planner_B.idx++) {

      cartesian_waypoints_planner_B.coffset = cartesian_waypoints_planner_B.idx *

        6 - 1;

      cartesian_waypoints_planner_B.boffset = cartesian_waypoints_planner_B.idx *

        AIn->size[0] - 1;

      for (cartesian_waypoints_planner_B.b_i_h = 0;

           cartesian_waypoints_planner_B.b_i_h < 6;

           cartesian_waypoints_planner_B.b_i_h++) {

        cartesian_waypoints_planner_B.s_i = 0.0;

        for (cartesian_waypoints_planner_B.g_idx_0 = 0;

             cartesian_waypoints_planner_B.g_idx_0 < 6;

             cartesian_waypoints_planner_B.g_idx_0++) {

          cartesian_waypoints_planner_B.s_i +=

            cartesian_waypoints_planner_B.V[cartesian_waypoints_planner_B.g_idx_0

            * 6 + cartesian_waypoints_planner_B.b_i_h] * AIn->data

            [(cartesian_waypoints_planner_B.boffset +

              cartesian_waypoints_planner_B.g_idx_0) + 1];

        }



        unusedU1->data[(cartesian_waypoints_planner_B.coffset +

                        cartesian_waypoints_planner_B.b_i_h) + 1] =

          cartesian_waypoints_planner_B.s_i;

      }

    }



    for (cartesian_waypoints_planner_B.b_i_h = 0;

         cartesian_waypoints_planner_B.b_i_h < 6;

         cartesian_waypoints_planner_B.b_i_h++) {

      for (cartesian_waypoints_planner_B.idx = 0;

           cartesian_waypoints_planner_B.idx < 6;

           cartesian_waypoints_planner_B.idx++) {

        cartesian_waypoints_planner_B.s_i = 0.0;

        for (cartesian_waypoints_planner_B.i_h = 0;

             cartesian_waypoints_planner_B.i_h < 6;

             cartesian_waypoints_planner_B.i_h++) {

          cartesian_waypoints_planner_B.s_i += unusedU1->data[6 *

            cartesian_waypoints_planner_B.i_h +

            cartesian_waypoints_planner_B.b_i_h] *

            cartesian_waypoints_planner_B.H[6 *

            cartesian_waypoints_planner_B.idx +

            cartesian_waypoints_planner_B.i_h];

        }



        cartesian_waypoints_planner_B.idxl = 6 *

          cartesian_waypoints_planner_B.idx +

          cartesian_waypoints_planner_B.b_i_h;

        cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.idxl] =

          cartesian_waypoints_planner_B.H[cartesian_waypoints_planner_B.idxl] -

          cartesian_waypoints_planner_B.s_i;

      }

    }



    memcpy(&cartesian_waypoints_planner_B.H[0],

           &cartesian_waypoints_planner_B.H_m[0], 36U * sizeof(real_T));

  }



  cartesian_waypoi_emxFree_real_T(&A_1);

  cartesian_waypoi_emxFree_real_T(&A_0);

  for (cartesian_waypoints_planner_B.i_h = 0; cartesian_waypoints_planner_B.i_h <

       6; cartesian_waypoints_planner_B.i_h++) {

    xSol[cartesian_waypoints_planner_B.i_h] =

      cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.i_h];

  }



  cartesian_waypoints_planner_B.A_d = obj->MaxNumIterationInternal;

  cartesian_waypoints_planner_B.g_idx_0 = 0;

  cartesian_waypoi_emxInit_real_T(&L, 1);

  cartesian_waypo_emxInit_int32_T(&cb, 1);

  cartesian_waypo_emxInit_int32_T(&db, 1);

  cartesian_waypo_emxInit_int32_T(&eb, 1);

  cartesian_waypo_emxInit_int32_T(&fb, 1);

  cartesian_waypo_emxInit_int32_T(&gb, 1);

  cartesian_waypoi_emxInit_real_T(&a, 2);

  cartesian_waypoi_emxInit_real_T(&y, 1);

  cartesian_waypo_emxInit_int32_T(&ii_0, 1);

  cartesian_waypoi_emxInit_real_T(&y_0, 2);

  cartesian_way_emxInit_boolean_T(&x, 1);

  cartesian_waypoi_emxInit_real_T(&A_2, 2);

  cartesian_waypoi_emxInit_real_T(&sigma, 2);

  cartesian_waypoi_emxInit_real_T(&tmp, 2);

  cartesian_waypoi_emxInit_real_T(&tmp_0, 2);

  cartesian_waypoi_emxInit_real_T(&grad_0, 2);

  cartesian_waypoi_emxInit_real_T(&sNew, 2);

  cartesian_waypo_emxInit_int32_T(&ii_1, 1);

  cartesian_waypo_emxInit_int32_T(&ii_2, 1);

  cartesian_waypoi_emxInit_real_T(&grad_1, 2);

  cartesian_waypoi_emxInit_real_T(&A_3, 2);

  cartesian_waypoi_emxInit_real_T(&alpha_0, 2);

  cartesian_waypo_emxInit_int32_T(&ii_3, 1);

  do {

    exitg2 = 0;

    if (cartesian_waypoints_planner_B.g_idx_0 <= static_cast<int32_T>

        (cartesian_waypoints_planner_B.A_d) - 1) {

      cartesian_waypoints_planner_B.s_i = SystemTimeProvider_getElapsedTi

        (&obj->TimeObjInternal);

      cartesian_waypoints_planner_B.flag = (cartesian_waypoints_planner_B.s_i >

        obj->MaxTimeInternal);

      if (cartesian_waypoints_planner_B.flag) {

        *exitFlag = TimeLimitExceeded;

        args = obj->ExtraArgs;

        for (cartesian_waypoints_planner_B.b_i_h = 0;

             cartesian_waypoints_planner_B.b_i_h < 36;

             cartesian_waypoints_planner_B.b_i_h++) {

          cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]

            = args->WeightMatrix[cartesian_waypoints_planner_B.b_i_h];

        }



        cartesian_waypoints_planner_B.b_i_h = grad->size[0];

        grad->size[0] = args->ErrTemp->size[0];

        cartes_emxEnsureCapacity_real_T(grad,

          cartesian_waypoints_planner_B.b_i_h);

        cartesian_waypoints_planner_B.i_h = args->ErrTemp->size[0];

        for (cartesian_waypoints_planner_B.b_i_h = 0;

             cartesian_waypoints_planner_B.b_i_h <

             cartesian_waypoints_planner_B.i_h;

             cartesian_waypoints_planner_B.b_i_h++) {

          grad->data[cartesian_waypoints_planner_B.b_i_h] = args->ErrTemp->

            data[cartesian_waypoints_planner_B.b_i_h];

        }



        for (cartesian_waypoints_planner_B.b_i_h = 0;

             cartesian_waypoints_planner_B.b_i_h < 6;

             cartesian_waypoints_planner_B.b_i_h++) {

          cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h] =

            0.0;

          for (cartesian_waypoints_planner_B.idx = 0;

               cartesian_waypoints_planner_B.idx < 6;

               cartesian_waypoints_planner_B.idx++) {

            cartesian_waypoints_planner_B.A_d =

              cartesian_waypoints_planner_B.unusedU0[6 *

              cartesian_waypoints_planner_B.idx +

              cartesian_waypoints_planner_B.b_i_h] * grad->

              data[cartesian_waypoints_planner_B.idx] +

              cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];

            cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]

              = cartesian_waypoints_planner_B.A_d;

          }

        }



        *err = cartesian_waypoints_plan_norm_e(cartesian_waypoints_planner_B.x);

        *iter = static_cast<real_T>(cartesian_waypoints_planner_B.g_idx_0) + 1.0;

        exitg2 = 1;

      } else {

        if ((A->size[0] == 0) || (A->size[1] == 0)) {

          cartesian_waypoints_planner_B.b_i_h = alpha->size[0];

          alpha->size[0] = 1;

          cartes_emxEnsureCapacity_real_T(alpha,

            cartesian_waypoints_planner_B.b_i_h);

          alpha->data[0] = 0.0;

        } else {

          cartesian_waypoints_planner_B.m_ew = A->size[1] - 1;

          cartesian_waypoints_planner_B.inner = A->size[0] - 1;

          cartesian_waypoints_planner_B.n_n = A->size[1] - 1;

          cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];

          AIn->size[0] = A->size[1];

          AIn->size[1] = A->size[1];

          cartes_emxEnsureCapacity_real_T(AIn,

            cartesian_waypoints_planner_B.b_i_h);

          for (cartesian_waypoints_planner_B.idx = 0;

               cartesian_waypoints_planner_B.idx <=

               cartesian_waypoints_planner_B.n_n;

               cartesian_waypoints_planner_B.idx++) {

            cartesian_waypoints_planner_B.coffset =

              (cartesian_waypoints_planner_B.m_ew + 1) *

              cartesian_waypoints_planner_B.idx - 1;

            cartesian_waypoints_planner_B.boffset =

              cartesian_waypoints_planner_B.idx * A->size[0] - 1;

            for (cartesian_waypoints_planner_B.b_i_h = 0;

                 cartesian_waypoints_planner_B.b_i_h <=

                 cartesian_waypoints_planner_B.m_ew;

                 cartesian_waypoints_planner_B.b_i_h++) {

              AIn->data[(cartesian_waypoints_planner_B.coffset +

                         cartesian_waypoints_planner_B.b_i_h) + 1] = 0.0;

            }



            for (cartesian_waypoints_planner_B.nx_f = 0;

                 cartesian_waypoints_planner_B.nx_f <=

                 cartesian_waypoints_planner_B.inner;

                 cartesian_waypoints_planner_B.nx_f++) {

              cartesian_waypoints_planner_B.s_i = A->data

                [(cartesian_waypoints_planner_B.boffset +

                  cartesian_waypoints_planner_B.nx_f) + 1];

              for (cartesian_waypoints_planner_B.j_p = 0;

                   cartesian_waypoints_planner_B.j_p <=

                   cartesian_waypoints_planner_B.m_ew;

                   cartesian_waypoints_planner_B.j_p++) {

                cartesian_waypoints_planner_B.b_i_h =

                  (cartesian_waypoints_planner_B.coffset +

                   cartesian_waypoints_planner_B.j_p) + 1;

                AIn->data[cartesian_waypoints_planner_B.b_i_h] += A->

                  data[cartesian_waypoints_planner_B.j_p * A->size[0] +

                  cartesian_waypoints_planner_B.nx_f] *

                  cartesian_waypoints_planner_B.s_i;

              }

            }

          }



          cartesian_waypoints_planner_B.b_i_h = A_2->size[0] * A_2->size[1];

          A_2->size[0] = A->size[1];

          A_2->size[1] = A->size[0];

          cartes_emxEnsureCapacity_real_T(A_2,

            cartesian_waypoints_planner_B.b_i_h);

          cartesian_waypoints_planner_B.i_h = A->size[0];

          for (cartesian_waypoints_planner_B.b_i_h = 0;

               cartesian_waypoints_planner_B.b_i_h <

               cartesian_waypoints_planner_B.i_h;

               cartesian_waypoints_planner_B.b_i_h++) {

            cartesian_waypoints_planner_B.n_n = A->size[1];

            for (cartesian_waypoints_planner_B.idx = 0;

                 cartesian_waypoints_planner_B.idx <

                 cartesian_waypoints_planner_B.n_n;

                 cartesian_waypoints_planner_B.idx++) {

              A_2->data[cartesian_waypoints_planner_B.idx + A_2->size[0] *

                cartesian_waypoints_planner_B.b_i_h] = A->data[A->size[0] *

                cartesian_waypoints_planner_B.idx +

                cartesian_waypoints_planner_B.b_i_h];

            }

          }



          cartesian_waypoints_pl_mldivide(AIn, A_2, a);

          cartesian_waypoints_planner_B.m_ew = a->size[0] - 1;

          cartesian_waypoints_planner_B.inner = a->size[1] - 1;

          cartesian_waypoints_planner_B.b_i_h = alpha->size[0];

          alpha->size[0] = a->size[0];

          cartes_emxEnsureCapacity_real_T(alpha,

            cartesian_waypoints_planner_B.b_i_h);

          for (cartesian_waypoints_planner_B.b_i_h = 0;

               cartesian_waypoints_planner_B.b_i_h <=

               cartesian_waypoints_planner_B.m_ew;

               cartesian_waypoints_planner_B.b_i_h++) {

            alpha->data[cartesian_waypoints_planner_B.b_i_h] = 0.0;

          }



          for (cartesian_waypoints_planner_B.nx_f = 0;

               cartesian_waypoints_planner_B.nx_f <=

               cartesian_waypoints_planner_B.inner;

               cartesian_waypoints_planner_B.nx_f++) {

            cartesian_waypoints_planner_B.aoffset =

              cartesian_waypoints_planner_B.nx_f * a->size[0] - 1;

            for (cartesian_waypoints_planner_B.j_p = 0;

                 cartesian_waypoints_planner_B.j_p <=

                 cartesian_waypoints_planner_B.m_ew;

                 cartesian_waypoints_planner_B.j_p++) {

              alpha->data[cartesian_waypoints_planner_B.j_p] += a->data

                [(cartesian_waypoints_planner_B.aoffset +

                  cartesian_waypoints_planner_B.j_p) + 1] * grad->

                data[cartesian_waypoints_planner_B.nx_f];

            }

          }

        }



        for (cartesian_waypoints_planner_B.b_i_h = 0;

             cartesian_waypoints_planner_B.b_i_h < 6;

             cartesian_waypoints_planner_B.b_i_h++) {

          cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h] =

            0.0;

          for (cartesian_waypoints_planner_B.idx = 0;

               cartesian_waypoints_planner_B.idx < 6;

               cartesian_waypoints_planner_B.idx++) {

            cartesian_waypoints_planner_B.b_gamma =

              cartesian_waypoints_planner_B.H[6 *

              cartesian_waypoints_planner_B.idx +

              cartesian_waypoints_planner_B.b_i_h] * grad->

              data[cartesian_waypoints_planner_B.idx] +

              cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h];

            cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]

              = cartesian_waypoints_planner_B.b_gamma;

          }

        }



        if (DampedBFGSwGradientProjection_a(obj,

             cartesian_waypoints_planner_B.Hg, alpha)) {

          *exitFlag = LocalMinimumFound;

          args = obj->ExtraArgs;

          for (cartesian_waypoints_planner_B.b_i_h = 0;

               cartesian_waypoints_planner_B.b_i_h < 36;

               cartesian_waypoints_planner_B.b_i_h++) {

            cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]

              = args->WeightMatrix[cartesian_waypoints_planner_B.b_i_h];

          }



          cartesian_waypoints_planner_B.b_i_h = grad->size[0];

          grad->size[0] = args->ErrTemp->size[0];

          cartes_emxEnsureCapacity_real_T(grad,

            cartesian_waypoints_planner_B.b_i_h);

          cartesian_waypoints_planner_B.i_h = args->ErrTemp->size[0];

          for (cartesian_waypoints_planner_B.b_i_h = 0;

               cartesian_waypoints_planner_B.b_i_h <

               cartesian_waypoints_planner_B.i_h;

               cartesian_waypoints_planner_B.b_i_h++) {

            grad->data[cartesian_waypoints_planner_B.b_i_h] = args->

              ErrTemp->data[cartesian_waypoints_planner_B.b_i_h];

          }



          for (cartesian_waypoints_planner_B.b_i_h = 0;

               cartesian_waypoints_planner_B.b_i_h < 6;

               cartesian_waypoints_planner_B.b_i_h++) {

            cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]

              = 0.0;

            for (cartesian_waypoints_planner_B.idx = 0;

                 cartesian_waypoints_planner_B.idx < 6;

                 cartesian_waypoints_planner_B.idx++) {

              cartesian_waypoints_planner_B.A_d =

                cartesian_waypoints_planner_B.unusedU0[6 *

                cartesian_waypoints_planner_B.idx +

                cartesian_waypoints_planner_B.b_i_h] * grad->

                data[cartesian_waypoints_planner_B.idx] +

                cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];

              cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]

                = cartesian_waypoints_planner_B.A_d;

            }

          }



          *err = cartesian_waypoints_plan_norm_e(cartesian_waypoints_planner_B.x);

          *iter = static_cast<real_T>(cartesian_waypoints_planner_B.g_idx_0) +

            1.0;

          exitg2 = 1;

        } else {

          guard1 = false;

          guard2 = false;

          if (obj->ConstraintsOn && ((A->size[0] != 0) && (A->size[1] != 0))) {

            cartesian_waypoints_planner_B.m_ew = A->size[1] - 1;

            cartesian_waypoints_planner_B.inner = A->size[0] - 1;

            cartesian_waypoints_planner_B.n_n = A->size[1] - 1;

            cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];

            AIn->size[0] = A->size[1];

            AIn->size[1] = A->size[1];

            cartes_emxEnsureCapacity_real_T(AIn,

              cartesian_waypoints_planner_B.b_i_h);

            for (cartesian_waypoints_planner_B.idx = 0;

                 cartesian_waypoints_planner_B.idx <=

                 cartesian_waypoints_planner_B.n_n;

                 cartesian_waypoints_planner_B.idx++) {

              cartesian_waypoints_planner_B.coffset =

                (cartesian_waypoints_planner_B.m_ew + 1) *

                cartesian_waypoints_planner_B.idx - 1;

              cartesian_waypoints_planner_B.boffset =

                cartesian_waypoints_planner_B.idx * A->size[0] - 1;

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <=

                   cartesian_waypoints_planner_B.m_ew;

                   cartesian_waypoints_planner_B.b_i_h++) {

                AIn->data[(cartesian_waypoints_planner_B.coffset +

                           cartesian_waypoints_planner_B.b_i_h) + 1] = 0.0;

              }



              for (cartesian_waypoints_planner_B.nx_f = 0;

                   cartesian_waypoints_planner_B.nx_f <=

                   cartesian_waypoints_planner_B.inner;

                   cartesian_waypoints_planner_B.nx_f++) {

                cartesian_waypoints_planner_B.s_i = A->data

                  [(cartesian_waypoints_planner_B.boffset +

                    cartesian_waypoints_planner_B.nx_f) + 1];

                for (cartesian_waypoints_planner_B.j_p = 0;

                     cartesian_waypoints_planner_B.j_p <=

                     cartesian_waypoints_planner_B.m_ew;

                     cartesian_waypoints_planner_B.j_p++) {

                  cartesian_waypoints_planner_B.b_i_h =

                    (cartesian_waypoints_planner_B.coffset +

                     cartesian_waypoints_planner_B.j_p) + 1;

                  AIn->data[cartesian_waypoints_planner_B.b_i_h] += A->

                    data[cartesian_waypoints_planner_B.j_p * A->size[0] +

                    cartesian_waypoints_planner_B.nx_f] *

                    cartesian_waypoints_planner_B.s_i;

                }

              }

            }



            cartesian_waypoints_planner_inv(AIn, a);

            cartesian_waypoints_planne_diag(a, L);

            cartesian_waypoints_pla_sqrt_ev(L);

            cartesian_waypoints_planner_B.b_i_h = alpha->size[0];

            cartes_emxEnsureCapacity_real_T(alpha,

              cartesian_waypoints_planner_B.b_i_h);

            cartesian_waypoints_planner_B.i_h = alpha->size[0];

            for (cartesian_waypoints_planner_B.b_i_h = 0;

                 cartesian_waypoints_planner_B.b_i_h <

                 cartesian_waypoints_planner_B.i_h;

                 cartesian_waypoints_planner_B.b_i_h++) {

              alpha->data[cartesian_waypoints_planner_B.b_i_h] /= L->

                data[cartesian_waypoints_planner_B.b_i_h];

            }



            cartesian_waypoints_planner_B.n_n = alpha->size[0];

            if (alpha->size[0] <= 2) {

              if (alpha->size[0] == 1) {

                cartesian_waypoints_planner_B.s_i = alpha->data[0];

                cartesian_waypoints_planner_B.idxl = 0;

              } else if ((alpha->data[0] < alpha->data[1]) || (rtIsNaN

                          (alpha->data[0]) && (!rtIsNaN(alpha->data[1])))) {

                cartesian_waypoints_planner_B.s_i = alpha->data[1];

                cartesian_waypoints_planner_B.idxl = 1;

              } else {

                cartesian_waypoints_planner_B.s_i = alpha->data[0];

                cartesian_waypoints_planner_B.idxl = 0;

              }

            } else {

              if (!rtIsNaN(alpha->data[0])) {

                cartesian_waypoints_planner_B.idxl = 1;

              } else {

                cartesian_waypoints_planner_B.idxl = 0;

                cartesian_waypoints_planner_B.b_i_h = 2;

                exitg3 = false;

                while ((!exitg3) && (cartesian_waypoints_planner_B.b_i_h <=

                                     alpha->size[0])) {

                  if (!rtIsNaN(alpha->data[cartesian_waypoints_planner_B.b_i_h -

                               1])) {

                    cartesian_waypoints_planner_B.idxl =

                      cartesian_waypoints_planner_B.b_i_h;

                    exitg3 = true;

                  } else {

                    cartesian_waypoints_planner_B.b_i_h++;

                  }

                }

              }



              if (cartesian_waypoints_planner_B.idxl == 0) {

                cartesian_waypoints_planner_B.s_i = alpha->data[0];

              } else {

                cartesian_waypoints_planner_B.s_i = alpha->

                  data[cartesian_waypoints_planner_B.idxl - 1];

                cartesian_waypoints_planner_B.nx_f =

                  cartesian_waypoints_planner_B.idxl;

                for (cartesian_waypoints_planner_B.b_i_h =

                     cartesian_waypoints_planner_B.idxl + 1;

                     cartesian_waypoints_planner_B.b_i_h <=

                     cartesian_waypoints_planner_B.n_n;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  if (cartesian_waypoints_planner_B.s_i < alpha->

                      data[cartesian_waypoints_planner_B.b_i_h - 1]) {

                    cartesian_waypoints_planner_B.s_i = alpha->

                      data[cartesian_waypoints_planner_B.b_i_h - 1];

                    cartesian_waypoints_planner_B.nx_f =

                      cartesian_waypoints_planner_B.b_i_h;

                  }

                }



                cartesian_waypoints_planner_B.idxl =

                  cartesian_waypoints_planner_B.nx_f - 1;

              }

            }



            if (cartesian_waypoints_plan_norm_e(cartesian_waypoints_planner_B.Hg)

                < 0.5 * cartesian_waypoints_planner_B.s_i) {

              cartesian_waypoints_planner_B.nx_f = activeSet->size[0];

              cartesian_waypoints_planner_B.idx = 0;

              cartesian_waypoints_planner_B.b_i_h = ii->size[0];

              ii->size[0] = activeSet->size[0];

              carte_emxEnsureCapacity_int32_T(ii,

                cartesian_waypoints_planner_B.b_i_h);

              cartesian_waypoints_planner_B.b_i_h = 1;

              exitg3 = false;

              while ((!exitg3) && (cartesian_waypoints_planner_B.b_i_h - 1 <=

                                   cartesian_waypoints_planner_B.nx_f - 1)) {

                if (activeSet->data[cartesian_waypoints_planner_B.b_i_h - 1]) {

                  cartesian_waypoints_planner_B.idx++;

                  ii->data[cartesian_waypoints_planner_B.idx - 1] =

                    cartesian_waypoints_planner_B.b_i_h;

                  if (cartesian_waypoints_planner_B.idx >=

                      cartesian_waypoints_planner_B.nx_f) {

                    exitg3 = true;

                  } else {

                    cartesian_waypoints_planner_B.b_i_h++;

                  }

                } else {

                  cartesian_waypoints_planner_B.b_i_h++;

                }

              }



              if (activeSet->size[0] == 1) {

                if (cartesian_waypoints_planner_B.idx == 0) {

                  ii->size[0] = 0;

                }

              } else {

                if (1 > cartesian_waypoints_planner_B.idx) {

                  cartesian_waypoints_planner_B.idx = 0;

                }



                cartesian_waypoints_planner_B.b_i_h = ii_1->size[0];

                ii_1->size[0] = cartesian_waypoints_planner_B.idx;

                carte_emxEnsureCapacity_int32_T(ii_1,

                  cartesian_waypoints_planner_B.b_i_h);

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h <

                     cartesian_waypoints_planner_B.idx;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  ii_1->data[cartesian_waypoints_planner_B.b_i_h] = ii->

                    data[cartesian_waypoints_planner_B.b_i_h];

                }



                cartesian_waypoints_planner_B.b_i_h = ii->size[0];

                ii->size[0] = ii_1->size[0];

                carte_emxEnsureCapacity_int32_T(ii,

                  cartesian_waypoints_planner_B.b_i_h);

                cartesian_waypoints_planner_B.i_h = ii_1->size[0];

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h <

                     cartesian_waypoints_planner_B.i_h;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  ii->data[cartesian_waypoints_planner_B.b_i_h] = ii_1->

                    data[cartesian_waypoints_planner_B.b_i_h];

                }

              }



              cartesian_waypoints_planner_B.b_i_h = alpha->size[0];

              alpha->size[0] = ii->size[0];

              cartes_emxEnsureCapacity_real_T(alpha,

                cartesian_waypoints_planner_B.b_i_h);

              cartesian_waypoints_planner_B.i_h = ii->size[0];

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <

                   cartesian_waypoints_planner_B.i_h;

                   cartesian_waypoints_planner_B.b_i_h++) {

                alpha->data[cartesian_waypoints_planner_B.b_i_h] = ii->

                  data[cartesian_waypoints_planner_B.b_i_h];

              }



              activeSet->data[static_cast<int32_T>(alpha->

                data[cartesian_waypoints_planner_B.idxl]) - 1] = false;

              cartesian_waypoints_planner_B.nx_f = activeSet->size[0] - 1;

              cartesian_waypoints_planner_B.idx = 0;

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <=

                   cartesian_waypoints_planner_B.nx_f;

                   cartesian_waypoints_planner_B.b_i_h++) {

                if (activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {

                  cartesian_waypoints_planner_B.idx++;

                }

              }



              cartesian_waypoints_planner_B.b_i_h = eb->size[0];

              eb->size[0] = cartesian_waypoints_planner_B.idx;

              carte_emxEnsureCapacity_int32_T(eb,

                cartesian_waypoints_planner_B.b_i_h);

              cartesian_waypoints_planner_B.idx = 0;

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <=

                   cartesian_waypoints_planner_B.nx_f;

                   cartesian_waypoints_planner_B.b_i_h++) {

                if (activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {

                  eb->data[cartesian_waypoints_planner_B.idx] =

                    cartesian_waypoints_planner_B.b_i_h + 1;

                  cartesian_waypoints_planner_B.idx++;

                }

              }



              cartesian_waypoints_planner_B.i_h = obj->ConstraintMatrix->size[0];

              cartesian_waypoints_planner_B.b_i_h = A->size[0] * A->size[1];

              A->size[0] = cartesian_waypoints_planner_B.i_h;

              A->size[1] = eb->size[0];

              cartes_emxEnsureCapacity_real_T(A,

                cartesian_waypoints_planner_B.b_i_h);

              cartesian_waypoints_planner_B.n_n = eb->size[0];

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <

                   cartesian_waypoints_planner_B.n_n;

                   cartesian_waypoints_planner_B.b_i_h++) {

                for (cartesian_waypoints_planner_B.idx = 0;

                     cartesian_waypoints_planner_B.idx <

                     cartesian_waypoints_planner_B.i_h;

                     cartesian_waypoints_planner_B.idx++) {

                  A->data[cartesian_waypoints_planner_B.idx + A->size[0] *

                    cartesian_waypoints_planner_B.b_i_h] = obj->

                    ConstraintMatrix->data[(eb->

                    data[cartesian_waypoints_planner_B.b_i_h] - 1) *

                    obj->ConstraintMatrix->size[0] +

                    cartesian_waypoints_planner_B.idx];

                }

              }



              cartesian_waypoints_planner_B.m_ew = A->size[1] - 1;

              cartesian_waypoints_planner_B.inner = A->size[0] - 1;

              cartesian_waypoints_planner_B.n_n = A->size[1] - 1;

              cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];

              AIn->size[0] = A->size[1];

              AIn->size[1] = A->size[1];

              cartes_emxEnsureCapacity_real_T(AIn,

                cartesian_waypoints_planner_B.b_i_h);

              for (cartesian_waypoints_planner_B.idx = 0;

                   cartesian_waypoints_planner_B.idx <=

                   cartesian_waypoints_planner_B.n_n;

                   cartesian_waypoints_planner_B.idx++) {

                cartesian_waypoints_planner_B.coffset =

                  (cartesian_waypoints_planner_B.m_ew + 1) *

                  cartesian_waypoints_planner_B.idx - 1;

                cartesian_waypoints_planner_B.boffset =

                  cartesian_waypoints_planner_B.idx * A->size[0] - 1;

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h <=

                     cartesian_waypoints_planner_B.m_ew;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  AIn->data[(cartesian_waypoints_planner_B.coffset +

                             cartesian_waypoints_planner_B.b_i_h) + 1] = 0.0;

                }



                for (cartesian_waypoints_planner_B.nx_f = 0;

                     cartesian_waypoints_planner_B.nx_f <=

                     cartesian_waypoints_planner_B.inner;

                     cartesian_waypoints_planner_B.nx_f++) {

                  cartesian_waypoints_planner_B.s_i = A->data

                    [(cartesian_waypoints_planner_B.boffset +

                      cartesian_waypoints_planner_B.nx_f) + 1];

                  for (cartesian_waypoints_planner_B.j_p = 0;

                       cartesian_waypoints_planner_B.j_p <=

                       cartesian_waypoints_planner_B.m_ew;

                       cartesian_waypoints_planner_B.j_p++) {

                    cartesian_waypoints_planner_B.b_i_h =

                      (cartesian_waypoints_planner_B.coffset +

                       cartesian_waypoints_planner_B.j_p) + 1;

                    AIn->data[cartesian_waypoints_planner_B.b_i_h] += A->

                      data[cartesian_waypoints_planner_B.j_p * A->size[0] +

                      cartesian_waypoints_planner_B.nx_f] *

                      cartesian_waypoints_planner_B.s_i;

                  }

                }

              }



              cartesian_waypoints_planner_B.b_i_h = A_3->size[0] * A_3->size[1];

              A_3->size[0] = A->size[1];

              A_3->size[1] = A->size[0];

              cartes_emxEnsureCapacity_real_T(A_3,

                cartesian_waypoints_planner_B.b_i_h);

              cartesian_waypoints_planner_B.i_h = A->size[0];

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <

                   cartesian_waypoints_planner_B.i_h;

                   cartesian_waypoints_planner_B.b_i_h++) {

                cartesian_waypoints_planner_B.n_n = A->size[1];

                for (cartesian_waypoints_planner_B.idx = 0;

                     cartesian_waypoints_planner_B.idx <

                     cartesian_waypoints_planner_B.n_n;

                     cartesian_waypoints_planner_B.idx++) {

                  A_3->data[cartesian_waypoints_planner_B.idx + A_3->size[0] *

                    cartesian_waypoints_planner_B.b_i_h] = A->data[A->size[0] *

                    cartesian_waypoints_planner_B.idx +

                    cartesian_waypoints_planner_B.b_i_h];

                }

              }



              cartesian_waypoints_pl_mldivide(AIn, A_3, a);

              cartesian_waypoints_planner_B.m_ew = A->size[0] - 1;

              cartesian_waypoints_planner_B.inner = A->size[1] - 1;

              cartesian_waypoints_planner_B.n_n = a->size[1] - 1;

              cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];

              AIn->size[0] = A->size[0];

              AIn->size[1] = a->size[1];

              cartes_emxEnsureCapacity_real_T(AIn,

                cartesian_waypoints_planner_B.b_i_h);

              for (cartesian_waypoints_planner_B.idx = 0;

                   cartesian_waypoints_planner_B.idx <=

                   cartesian_waypoints_planner_B.n_n;

                   cartesian_waypoints_planner_B.idx++) {

                cartesian_waypoints_planner_B.coffset =

                  (cartesian_waypoints_planner_B.m_ew + 1) *

                  cartesian_waypoints_planner_B.idx - 1;

                cartesian_waypoints_planner_B.boffset =

                  cartesian_waypoints_planner_B.idx * a->size[0] - 1;

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h <=

                     cartesian_waypoints_planner_B.m_ew;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  AIn->data[(cartesian_waypoints_planner_B.coffset +

                             cartesian_waypoints_planner_B.b_i_h) + 1] = 0.0;

                }



                for (cartesian_waypoints_planner_B.nx_f = 0;

                     cartesian_waypoints_planner_B.nx_f <=

                     cartesian_waypoints_planner_B.inner;

                     cartesian_waypoints_planner_B.nx_f++) {

                  cartesian_waypoints_planner_B.aoffset =

                    cartesian_waypoints_planner_B.nx_f * A->size[0] - 1;

                  cartesian_waypoints_planner_B.s_i = a->data

                    [(cartesian_waypoints_planner_B.boffset +

                      cartesian_waypoints_planner_B.nx_f) + 1];

                  for (cartesian_waypoints_planner_B.j_p = 0;

                       cartesian_waypoints_planner_B.j_p <=

                       cartesian_waypoints_planner_B.m_ew;

                       cartesian_waypoints_planner_B.j_p++) {

                    cartesian_waypoints_planner_B.i_h =

                      cartesian_waypoints_planner_B.j_p + 1;

                    cartesian_waypoints_planner_B.b_i_h =

                      cartesian_waypoints_planner_B.coffset +

                      cartesian_waypoints_planner_B.i_h;

                    AIn->data[cartesian_waypoints_planner_B.b_i_h] += A->

                      data[cartesian_waypoints_planner_B.aoffset +

                      cartesian_waypoints_planner_B.i_h] *

                      cartesian_waypoints_planner_B.s_i;

                  }

                }

              }



              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h < 36;

                   cartesian_waypoints_planner_B.b_i_h++) {

                cartesian_waypoints_planner_B.P[cartesian_waypoints_planner_B.b_i_h]

                  =

                  cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]

                  - AIn->data[cartesian_waypoints_planner_B.b_i_h];

              }



              cartesian_waypoints_planner_B.s_i = alpha->

                data[cartesian_waypoints_planner_B.idxl];

              cartesian_waypoints_planner_B.b_i_h = static_cast<int32_T>

                (cartesian_waypoints_planner_B.s_i);

              cartesian_waypoints_planner_B.i_h = obj->ConstraintMatrix->size[0];

              cartesian_waypoints_planner_B.idx = alpha->size[0];

              alpha->size[0] = cartesian_waypoints_planner_B.i_h;

              cartes_emxEnsureCapacity_real_T(alpha,

                cartesian_waypoints_planner_B.idx);

              for (cartesian_waypoints_planner_B.idx = 0;

                   cartesian_waypoints_planner_B.idx <

                   cartesian_waypoints_planner_B.i_h;

                   cartesian_waypoints_planner_B.idx++) {

                alpha->data[cartesian_waypoints_planner_B.idx] =

                  obj->ConstraintMatrix->data

                  [(cartesian_waypoints_planner_B.b_i_h - 1) *

                  obj->ConstraintMatrix->size[0] +

                  cartesian_waypoints_planner_B.idx];

              }



              cartesian_waypoints_planner_B.b_i_h = alpha_0->size[0] *

                alpha_0->size[1];

              alpha_0->size[0] = 1;

              alpha_0->size[1] = alpha->size[0];

              cartes_emxEnsureCapacity_real_T(alpha_0,

                cartesian_waypoints_planner_B.b_i_h);

              cartesian_waypoints_planner_B.i_h = alpha->size[0];

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <

                   cartesian_waypoints_planner_B.i_h;

                   cartesian_waypoints_planner_B.b_i_h++) {

                alpha_0->data[cartesian_waypoints_planner_B.b_i_h] = alpha->

                  data[cartesian_waypoints_planner_B.b_i_h];

              }



              cartesian_waypoints_planner_B.s_i = 0.0;

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h < 6;

                   cartesian_waypoints_planner_B.b_i_h++) {

                cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]

                  = 0.0;

                for (cartesian_waypoints_planner_B.idx = 0;

                     cartesian_waypoints_planner_B.idx < 6;

                     cartesian_waypoints_planner_B.idx++) {

                  cartesian_waypoints_planner_B.b_gamma =

                    cartesian_waypoints_planner_B.P[6 *

                    cartesian_waypoints_planner_B.b_i_h +

                    cartesian_waypoints_planner_B.idx] * alpha_0->

                    data[cartesian_waypoints_planner_B.idx] +

                    cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h];

                  cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]

                    = cartesian_waypoints_planner_B.b_gamma;

                }



                cartesian_waypoints_planner_B.s_i +=

                  cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]

                  * alpha->data[cartesian_waypoints_planner_B.b_i_h];

              }



              cartesian_waypoints_planner_B.s_i = 1.0 /

                cartesian_waypoints_planner_B.s_i;

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h < 36;

                   cartesian_waypoints_planner_B.b_i_h++) {

                cartesian_waypoints_planner_B.V[cartesian_waypoints_planner_B.b_i_h]

                  = cartesian_waypoints_planner_B.s_i *

                  cartesian_waypoints_planner_B.P[cartesian_waypoints_planner_B.b_i_h];

              }



              cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];

              AIn->size[0] = alpha->size[0];

              AIn->size[1] = alpha->size[0];

              cartes_emxEnsureCapacity_real_T(AIn,

                cartesian_waypoints_planner_B.b_i_h);

              cartesian_waypoints_planner_B.i_h = alpha->size[0];

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <

                   cartesian_waypoints_planner_B.i_h;

                   cartesian_waypoints_planner_B.b_i_h++) {

                cartesian_waypoints_planner_B.n_n = alpha->size[0];

                for (cartesian_waypoints_planner_B.idx = 0;

                     cartesian_waypoints_planner_B.idx <

                     cartesian_waypoints_planner_B.n_n;

                     cartesian_waypoints_planner_B.idx++) {

                  AIn->data[cartesian_waypoints_planner_B.idx + AIn->size[0] *

                    cartesian_waypoints_planner_B.b_i_h] = alpha->

                    data[cartesian_waypoints_planner_B.idx] * alpha->

                    data[cartesian_waypoints_planner_B.b_i_h];

                }

              }



              cartesian_waypoints_planner_B.n_n = AIn->size[1] - 1;

              cartesian_waypoints_planner_B.b_i_h = unusedU1->size[0] *

                unusedU1->size[1];

              unusedU1->size[0] = 6;

              unusedU1->size[1] = AIn->size[1];

              cartes_emxEnsureCapacity_real_T(unusedU1,

                cartesian_waypoints_planner_B.b_i_h);

              for (cartesian_waypoints_planner_B.idx = 0;

                   cartesian_waypoints_planner_B.idx <=

                   cartesian_waypoints_planner_B.n_n;

                   cartesian_waypoints_planner_B.idx++) {

                cartesian_waypoints_planner_B.coffset =

                  cartesian_waypoints_planner_B.idx * 6 - 1;

                cartesian_waypoints_planner_B.boffset =

                  cartesian_waypoints_planner_B.idx * AIn->size[0] - 1;

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h < 6;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  cartesian_waypoints_planner_B.s_i = 0.0;

                  for (cartesian_waypoints_planner_B.nx_f = 0;

                       cartesian_waypoints_planner_B.nx_f < 6;

                       cartesian_waypoints_planner_B.nx_f++) {

                    cartesian_waypoints_planner_B.s_i +=

                      cartesian_waypoints_planner_B.V[cartesian_waypoints_planner_B.nx_f

                      * 6 + cartesian_waypoints_planner_B.b_i_h] * AIn->data

                      [(cartesian_waypoints_planner_B.boffset +

                        cartesian_waypoints_planner_B.nx_f) + 1];

                  }



                  unusedU1->data[(cartesian_waypoints_planner_B.coffset +

                                  cartesian_waypoints_planner_B.b_i_h) + 1] =

                    cartesian_waypoints_planner_B.s_i;

                }

              }



              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h < 6;

                   cartesian_waypoints_planner_B.b_i_h++) {

                for (cartesian_waypoints_planner_B.idx = 0;

                     cartesian_waypoints_planner_B.idx < 6;

                     cartesian_waypoints_planner_B.idx++) {

                  cartesian_waypoints_planner_B.s_i = 0.0;

                  for (cartesian_waypoints_planner_B.i_h = 0;

                       cartesian_waypoints_planner_B.i_h < 6;

                       cartesian_waypoints_planner_B.i_h++) {

                    cartesian_waypoints_planner_B.s_i += unusedU1->data[6 *

                      cartesian_waypoints_planner_B.i_h +

                      cartesian_waypoints_planner_B.b_i_h] *

                      cartesian_waypoints_planner_B.P[6 *

                      cartesian_waypoints_planner_B.idx +

                      cartesian_waypoints_planner_B.i_h];

                  }



                  cartesian_waypoints_planner_B.idxl = 6 *

                    cartesian_waypoints_planner_B.idx +

                    cartesian_waypoints_planner_B.b_i_h;

                  cartesian_waypoints_planner_B.H[cartesian_waypoints_planner_B.idxl]

                    += cartesian_waypoints_planner_B.s_i;

                }

              }



              cartesian_waypoints_planner_B.g_idx_0++;

            } else {

              guard2 = true;

            }

          } else {

            guard2 = true;

          }



          if (guard2) {

            for (cartesian_waypoints_planner_B.b_i_h = 0;

                 cartesian_waypoints_planner_B.b_i_h < 6;

                 cartesian_waypoints_planner_B.b_i_h++) {

              cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]

                =

                -cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h];

            }



            cartesian_waypoints_planner_B.idxl = -1;

            if (obj->ConstraintsOn) {

              cartesian_waypoints_planner_B.b_i_h = x->size[0];

              x->size[0] = activeSet->size[0];

              car_emxEnsureCapacity_boolean_T(x,

                cartesian_waypoints_planner_B.b_i_h);

              cartesian_waypoints_planner_B.i_h = activeSet->size[0];

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <

                   cartesian_waypoints_planner_B.i_h;

                   cartesian_waypoints_planner_B.b_i_h++) {

                x->data[cartesian_waypoints_planner_B.b_i_h] = !activeSet->

                  data[cartesian_waypoints_planner_B.b_i_h];

              }



              if (cartesian_waypoints_planner_any(x)) {

                cartesian_waypoints_planner_B.nx_f = activeSet->size[0] - 1;

                cartesian_waypoints_planner_B.idx = 0;

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h <=

                     cartesian_waypoints_planner_B.nx_f;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  if (!activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {

                    cartesian_waypoints_planner_B.idx++;

                  }

                }



                cartesian_waypoints_planner_B.b_i_h = cb->size[0];

                cb->size[0] = cartesian_waypoints_planner_B.idx;

                carte_emxEnsureCapacity_int32_T(cb,

                  cartesian_waypoints_planner_B.b_i_h);

                cartesian_waypoints_planner_B.idx = 0;

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h <=

                     cartesian_waypoints_planner_B.nx_f;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  if (!activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {

                    cb->data[cartesian_waypoints_planner_B.idx] =

                      cartesian_waypoints_planner_B.b_i_h + 1;

                    cartesian_waypoints_planner_B.idx++;

                  }

                }



                cartesian_waypoints_planner_B.b_i_h = alpha->size[0];

                alpha->size[0] = cb->size[0];

                cartes_emxEnsureCapacity_real_T(alpha,

                  cartesian_waypoints_planner_B.b_i_h);

                cartesian_waypoints_planner_B.i_h = cb->size[0];

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h <

                     cartesian_waypoints_planner_B.i_h;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  alpha->data[cartesian_waypoints_planner_B.b_i_h] =

                    obj->ConstraintBound->data[cb->

                    data[cartesian_waypoints_planner_B.b_i_h] - 1];

                }



                cartesian_waypoints_planner_B.nx_f = activeSet->size[0] - 1;

                cartesian_waypoints_planner_B.idx = 0;

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h <=

                     cartesian_waypoints_planner_B.nx_f;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  if (!activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {

                    cartesian_waypoints_planner_B.idx++;

                  }

                }



                cartesian_waypoints_planner_B.b_i_h = db->size[0];

                db->size[0] = cartesian_waypoints_planner_B.idx;

                carte_emxEnsureCapacity_int32_T(db,

                  cartesian_waypoints_planner_B.b_i_h);

                cartesian_waypoints_planner_B.idx = 0;

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h <=

                     cartesian_waypoints_planner_B.nx_f;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  if (!activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {

                    db->data[cartesian_waypoints_planner_B.idx] =

                      cartesian_waypoints_planner_B.b_i_h + 1;

                    cartesian_waypoints_planner_B.idx++;

                  }

                }



                cartesian_waypoints_planner_B.i_h = obj->ConstraintMatrix->size

                  [0];

                cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];

                AIn->size[0] = cartesian_waypoints_planner_B.i_h;

                AIn->size[1] = db->size[0];

                cartes_emxEnsureCapacity_real_T(AIn,

                  cartesian_waypoints_planner_B.b_i_h);

                cartesian_waypoints_planner_B.n_n = db->size[0];

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h <

                     cartesian_waypoints_planner_B.n_n;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  for (cartesian_waypoints_planner_B.idx = 0;

                       cartesian_waypoints_planner_B.idx <

                       cartesian_waypoints_planner_B.i_h;

                       cartesian_waypoints_planner_B.idx++) {

                    AIn->data[cartesian_waypoints_planner_B.idx + AIn->size[0] *

                      cartesian_waypoints_planner_B.b_i_h] =

                      obj->ConstraintMatrix->data[(db->

                      data[cartesian_waypoints_planner_B.b_i_h] - 1) *

                      obj->ConstraintMatrix->size[0] +

                      cartesian_waypoints_planner_B.idx];

                  }

                }



                cartesian_waypoints_planner_B.nx_f = x->size[0];

                cartesian_waypoints_planner_B.idx = 0;

                cartesian_waypoints_planner_B.b_i_h = ii->size[0];

                ii->size[0] = x->size[0];

                carte_emxEnsureCapacity_int32_T(ii,

                  cartesian_waypoints_planner_B.b_i_h);

                cartesian_waypoints_planner_B.b_i_h = 1;

                exitg3 = false;

                while ((!exitg3) && (cartesian_waypoints_planner_B.b_i_h - 1 <=

                                     cartesian_waypoints_planner_B.nx_f - 1)) {

                  if (x->data[cartesian_waypoints_planner_B.b_i_h - 1]) {

                    cartesian_waypoints_planner_B.idx++;

                    ii->data[cartesian_waypoints_planner_B.idx - 1] =

                      cartesian_waypoints_planner_B.b_i_h;

                    if (cartesian_waypoints_planner_B.idx >=

                        cartesian_waypoints_planner_B.nx_f) {

                      exitg3 = true;

                    } else {

                      cartesian_waypoints_planner_B.b_i_h++;

                    }

                  } else {

                    cartesian_waypoints_planner_B.b_i_h++;

                  }

                }



                if (x->size[0] == 1) {

                  if (cartesian_waypoints_planner_B.idx == 0) {

                    ii->size[0] = 0;

                  }

                } else {

                  if (1 > cartesian_waypoints_planner_B.idx) {

                    cartesian_waypoints_planner_B.idx = 0;

                  }



                  cartesian_waypoints_planner_B.b_i_h = ii_2->size[0];

                  ii_2->size[0] = cartesian_waypoints_planner_B.idx;

                  carte_emxEnsureCapacity_int32_T(ii_2,

                    cartesian_waypoints_planner_B.b_i_h);

                  for (cartesian_waypoints_planner_B.b_i_h = 0;

                       cartesian_waypoints_planner_B.b_i_h <

                       cartesian_waypoints_planner_B.idx;

                       cartesian_waypoints_planner_B.b_i_h++) {

                    ii_2->data[cartesian_waypoints_planner_B.b_i_h] = ii->

                      data[cartesian_waypoints_planner_B.b_i_h];

                  }



                  cartesian_waypoints_planner_B.b_i_h = ii->size[0];

                  ii->size[0] = ii_2->size[0];

                  carte_emxEnsureCapacity_int32_T(ii,

                    cartesian_waypoints_planner_B.b_i_h);

                  cartesian_waypoints_planner_B.i_h = ii_2->size[0];

                  for (cartesian_waypoints_planner_B.b_i_h = 0;

                       cartesian_waypoints_planner_B.b_i_h <

                       cartesian_waypoints_planner_B.i_h;

                       cartesian_waypoints_planner_B.b_i_h++) {

                    ii->data[cartesian_waypoints_planner_B.b_i_h] = ii_2->

                      data[cartesian_waypoints_planner_B.b_i_h];

                  }

                }



                cartesian_waypoints_planner_B.m_ew = AIn->size[1] - 1;

                cartesian_waypoints_planner_B.inner = AIn->size[0] - 1;

                cartesian_waypoints_planner_B.b_i_h = L->size[0];

                L->size[0] = AIn->size[1];

                cartes_emxEnsureCapacity_real_T(L,

                  cartesian_waypoints_planner_B.b_i_h);

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h <=

                     cartesian_waypoints_planner_B.m_ew;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  L->data[cartesian_waypoints_planner_B.b_i_h] = 0.0;

                }



                for (cartesian_waypoints_planner_B.nx_f = 0;

                     cartesian_waypoints_planner_B.nx_f <=

                     cartesian_waypoints_planner_B.inner;

                     cartesian_waypoints_planner_B.nx_f++) {

                  for (cartesian_waypoints_planner_B.j_p = 0;

                       cartesian_waypoints_planner_B.j_p <=

                       cartesian_waypoints_planner_B.m_ew;

                       cartesian_waypoints_planner_B.j_p++) {

                    L->data[cartesian_waypoints_planner_B.j_p] += AIn->

                      data[cartesian_waypoints_planner_B.j_p * AIn->size[0] +

                      cartesian_waypoints_planner_B.nx_f] *

                      cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.nx_f];

                  }

                }



                cartesian_waypoints_planner_B.m_ew = AIn->size[1] - 1;

                cartesian_waypoints_planner_B.inner = AIn->size[0] - 1;

                cartesian_waypoints_planner_B.b_i_h = y->size[0];

                y->size[0] = AIn->size[1];

                cartes_emxEnsureCapacity_real_T(y,

                  cartesian_waypoints_planner_B.b_i_h);

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h <=

                     cartesian_waypoints_planner_B.m_ew;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  y->data[cartesian_waypoints_planner_B.b_i_h] = 0.0;

                }



                for (cartesian_waypoints_planner_B.nx_f = 0;

                     cartesian_waypoints_planner_B.nx_f <=

                     cartesian_waypoints_planner_B.inner;

                     cartesian_waypoints_planner_B.nx_f++) {

                  for (cartesian_waypoints_planner_B.j_p = 0;

                       cartesian_waypoints_planner_B.j_p <=

                       cartesian_waypoints_planner_B.m_ew;

                       cartesian_waypoints_planner_B.j_p++) {

                    y->data[cartesian_waypoints_planner_B.j_p] += AIn->

                      data[cartesian_waypoints_planner_B.j_p * AIn->size[0] +

                      cartesian_waypoints_planner_B.nx_f] *

                      cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.nx_f];

                  }

                }



                cartesian_waypoints_planner_B.b_i_h = alpha->size[0];

                cartes_emxEnsureCapacity_real_T(alpha,

                  cartesian_waypoints_planner_B.b_i_h);

                cartesian_waypoints_planner_B.i_h = alpha->size[0];

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h <

                     cartesian_waypoints_planner_B.i_h;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  alpha->data[cartesian_waypoints_planner_B.b_i_h] =

                    (alpha->data[cartesian_waypoints_planner_B.b_i_h] - L->

                     data[cartesian_waypoints_planner_B.b_i_h]) / y->

                    data[cartesian_waypoints_planner_B.b_i_h];

                }



                cartesian_waypoints_planner_B.b_i_h = x->size[0];

                x->size[0] = alpha->size[0];

                car_emxEnsureCapacity_boolean_T(x,

                  cartesian_waypoints_planner_B.b_i_h);

                cartesian_waypoints_planner_B.i_h = alpha->size[0];

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h <

                     cartesian_waypoints_planner_B.i_h;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  x->data[cartesian_waypoints_planner_B.b_i_h] = (alpha->

                    data[cartesian_waypoints_planner_B.b_i_h] > 0.0);

                }



                cartesian_waypoints_planner_B.nx_f = x->size[0];

                cartesian_waypoints_planner_B.idx = 0;

                cartesian_waypoints_planner_B.b_i_h = ii_0->size[0];

                ii_0->size[0] = x->size[0];

                carte_emxEnsureCapacity_int32_T(ii_0,

                  cartesian_waypoints_planner_B.b_i_h);

                cartesian_waypoints_planner_B.b_i_h = 1;

                exitg3 = false;

                while ((!exitg3) && (cartesian_waypoints_planner_B.b_i_h - 1 <=

                                     cartesian_waypoints_planner_B.nx_f - 1)) {

                  if (x->data[cartesian_waypoints_planner_B.b_i_h - 1]) {

                    cartesian_waypoints_planner_B.idx++;

                    ii_0->data[cartesian_waypoints_planner_B.idx - 1] =

                      cartesian_waypoints_planner_B.b_i_h;

                    if (cartesian_waypoints_planner_B.idx >=

                        cartesian_waypoints_planner_B.nx_f) {

                      exitg3 = true;

                    } else {

                      cartesian_waypoints_planner_B.b_i_h++;

                    }

                  } else {

                    cartesian_waypoints_planner_B.b_i_h++;

                  }

                }



                if (x->size[0] == 1) {

                  if (cartesian_waypoints_planner_B.idx == 0) {

                    ii_0->size[0] = 0;

                  }

                } else {

                  if (1 > cartesian_waypoints_planner_B.idx) {

                    cartesian_waypoints_planner_B.idx = 0;

                  }



                  cartesian_waypoints_planner_B.b_i_h = ii_3->size[0];

                  ii_3->size[0] = cartesian_waypoints_planner_B.idx;

                  carte_emxEnsureCapacity_int32_T(ii_3,

                    cartesian_waypoints_planner_B.b_i_h);

                  for (cartesian_waypoints_planner_B.b_i_h = 0;

                       cartesian_waypoints_planner_B.b_i_h <

                       cartesian_waypoints_planner_B.idx;

                       cartesian_waypoints_planner_B.b_i_h++) {

                    ii_3->data[cartesian_waypoints_planner_B.b_i_h] = ii_0->

                      data[cartesian_waypoints_planner_B.b_i_h];

                  }



                  cartesian_waypoints_planner_B.b_i_h = ii_0->size[0];

                  ii_0->size[0] = ii_3->size[0];

                  carte_emxEnsureCapacity_int32_T(ii_0,

                    cartesian_waypoints_planner_B.b_i_h);

                  cartesian_waypoints_planner_B.i_h = ii_3->size[0];

                  for (cartesian_waypoints_planner_B.b_i_h = 0;

                       cartesian_waypoints_planner_B.b_i_h <

                       cartesian_waypoints_planner_B.i_h;

                       cartesian_waypoints_planner_B.b_i_h++) {

                    ii_0->data[cartesian_waypoints_planner_B.b_i_h] = ii_3->

                      data[cartesian_waypoints_planner_B.b_i_h];

                  }

                }



                cartesian_waypoints_planner_B.b_i_h = L->size[0];

                L->size[0] = ii_0->size[0];

                cartes_emxEnsureCapacity_real_T(L,

                  cartesian_waypoints_planner_B.b_i_h);

                cartesian_waypoints_planner_B.i_h = ii_0->size[0];

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h <

                     cartesian_waypoints_planner_B.i_h;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  L->data[cartesian_waypoints_planner_B.b_i_h] = ii_0->

                    data[cartesian_waypoints_planner_B.b_i_h];

                }



                if (L->size[0] != 0) {

                  cartesian_waypoints_planner_B.nx_f = alpha->size[0] - 1;

                  cartesian_waypoints_planner_B.idx = 0;

                  for (cartesian_waypoints_planner_B.b_i_h = 0;

                       cartesian_waypoints_planner_B.b_i_h <=

                       cartesian_waypoints_planner_B.nx_f;

                       cartesian_waypoints_planner_B.b_i_h++) {

                    if (alpha->data[cartesian_waypoints_planner_B.b_i_h] > 0.0)

                    {

                      cartesian_waypoints_planner_B.idx++;

                    }

                  }



                  cartesian_waypoints_planner_B.b_i_h = fb->size[0];

                  fb->size[0] = cartesian_waypoints_planner_B.idx;

                  carte_emxEnsureCapacity_int32_T(fb,

                    cartesian_waypoints_planner_B.b_i_h);

                  cartesian_waypoints_planner_B.idx = 0;

                  for (cartesian_waypoints_planner_B.b_i_h = 0;

                       cartesian_waypoints_planner_B.b_i_h <=

                       cartesian_waypoints_planner_B.nx_f;

                       cartesian_waypoints_planner_B.b_i_h++) {

                    if (alpha->data[cartesian_waypoints_planner_B.b_i_h] > 0.0)

                    {

                      fb->data[cartesian_waypoints_planner_B.idx] =

                        cartesian_waypoints_planner_B.b_i_h + 1;

                      cartesian_waypoints_planner_B.idx++;

                    }

                  }



                  cartesian_waypoints_planner_B.n_n = fb->size[0];

                  if (fb->size[0] <= 2) {

                    if (fb->size[0] == 1) {

                      cartesian_waypoints_planner_B.s_i = alpha->data[fb->data[0]

                        - 1];

                      cartesian_waypoints_planner_B.idxl = 0;

                    } else if ((alpha->data[fb->data[0] - 1] > alpha->data

                                [fb->data[1] - 1]) || (rtIsNaN(alpha->data

                                 [fb->data[0] - 1]) && (!rtIsNaN(alpha->data

                                  [fb->data[1] - 1])))) {

                      cartesian_waypoints_planner_B.s_i = alpha->data[fb->data[1]

                        - 1];

                      cartesian_waypoints_planner_B.idxl = 1;

                    } else {

                      cartesian_waypoints_planner_B.s_i = alpha->data[fb->data[0]

                        - 1];

                      cartesian_waypoints_planner_B.idxl = 0;

                    }

                  } else {

                    if (!rtIsNaN(alpha->data[fb->data[0] - 1])) {

                      cartesian_waypoints_planner_B.idxl = 1;

                    } else {

                      cartesian_waypoints_planner_B.idxl = 0;

                      cartesian_waypoints_planner_B.b_i_h = 2;

                      exitg3 = false;

                      while ((!exitg3) && (cartesian_waypoints_planner_B.b_i_h <=

                                           fb->size[0])) {

                        if (!rtIsNaN(alpha->data[fb->

                                     data[cartesian_waypoints_planner_B.b_i_h -

                                     1] - 1])) {

                          cartesian_waypoints_planner_B.idxl =

                            cartesian_waypoints_planner_B.b_i_h;

                          exitg3 = true;

                        } else {

                          cartesian_waypoints_planner_B.b_i_h++;

                        }

                      }

                    }



                    if (cartesian_waypoints_planner_B.idxl == 0) {

                      cartesian_waypoints_planner_B.s_i = alpha->data[fb->data[0]

                        - 1];

                    } else {

                      cartesian_waypoints_planner_B.s_i = alpha->data[fb->

                        data[cartesian_waypoints_planner_B.idxl - 1] - 1];

                      cartesian_waypoints_planner_B.nx_f =

                        cartesian_waypoints_planner_B.idxl;

                      for (cartesian_waypoints_planner_B.b_i_h =

                           cartesian_waypoints_planner_B.idxl + 1;

                           cartesian_waypoints_planner_B.b_i_h <=

                           cartesian_waypoints_planner_B.n_n;

                           cartesian_waypoints_planner_B.b_i_h++) {

                        if (cartesian_waypoints_planner_B.s_i > alpha->data

                            [fb->data[cartesian_waypoints_planner_B.b_i_h - 1] -

                            1]) {

                          cartesian_waypoints_planner_B.s_i = alpha->data

                            [fb->data[cartesian_waypoints_planner_B.b_i_h - 1] -

                            1];

                          cartesian_waypoints_planner_B.nx_f =

                            cartesian_waypoints_planner_B.b_i_h;

                        }

                      }



                      cartesian_waypoints_planner_B.idxl =

                        cartesian_waypoints_planner_B.nx_f - 1;

                    }

                  }



                  cartesian_waypoints_planner_B.idxl = ii->data

                    [static_cast<int32_T>(L->

                    data[cartesian_waypoints_planner_B.idxl]) - 1];

                } else {

                  cartesian_waypoints_planner_B.s_i = 0.0;

                }

              } else {

                cartesian_waypoints_planner_B.s_i = 0.0;

              }

            } else {

              cartesian_waypoints_planner_B.s_i = 0.0;

            }



            if (cartesian_waypoints_planner_B.s_i > 0.0) {

              if (1.0 < cartesian_waypoints_planner_B.s_i) {

                cartesian_waypoints_planner_B.b_gamma = 1.0;

              } else {

                cartesian_waypoints_planner_B.b_gamma =

                  cartesian_waypoints_planner_B.s_i;

              }

            } else {

              cartesian_waypoints_planner_B.b_gamma = 1.0;

            }



            cartesian_waypoints_planner_B.beta = obj->ArmijoRuleBeta;

            cartesian_waypoints_planner_B.sigma = obj->ArmijoRuleSigma;

            for (cartesian_waypoints_planner_B.b_i_h = 0;

                 cartesian_waypoints_planner_B.b_i_h < 6;

                 cartesian_waypoints_planner_B.b_i_h++) {

              cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]

                = cartesian_waypoints_planner_B.b_gamma *

                cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]

                + cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];

            }



            cartesian_IKHelpers_computeCost(cartesian_waypoints_planner_B.sNew_a,

              obj->ExtraArgs, &cartesian_waypoints_planner_B.costNew,

              cartesian_waypoints_planner_B.V, unusedU1, &c);

            obj->ExtraArgs = c;

            cartesian_waypoints_planner_B.m = 0.0;

            do {

              exitg1 = 0;

              for (cartesian_waypoints_planner_B.i_h = 0;

                   cartesian_waypoints_planner_B.i_h < 6;

                   cartesian_waypoints_planner_B.i_h++) {

                xSol[cartesian_waypoints_planner_B.i_h] =

                  cartesian_waypoints_planner_B.b_gamma *

                  cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.i_h];

              }



              cartesian_waypoints_planner_B.b_i_h = sigma->size[0] * sigma->

                size[1];

              sigma->size[0] = 1;

              sigma->size[1] = grad->size[0];

              cartes_emxEnsureCapacity_real_T(sigma,

                cartesian_waypoints_planner_B.b_i_h);

              cartesian_waypoints_planner_B.i_h = grad->size[0];

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <

                   cartesian_waypoints_planner_B.i_h;

                   cartesian_waypoints_planner_B.b_i_h++) {

                sigma->data[cartesian_waypoints_planner_B.b_i_h] =

                  -cartesian_waypoints_planner_B.sigma * grad->

                  data[cartesian_waypoints_planner_B.b_i_h];

              }



              cartesian_waypoints_planner_B.sigma_g = 0.0;

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h < 6;

                   cartesian_waypoints_planner_B.b_i_h++) {

                cartesian_waypoints_planner_B.sigma_g += sigma->

                  data[cartesian_waypoints_planner_B.b_i_h] *

                  xSol[cartesian_waypoints_planner_B.b_i_h];

              }



              if (cartesian_waypoints_planner_B.cost -

                  cartesian_waypoints_planner_B.costNew <

                  cartesian_waypoints_planner_B.sigma_g) {

                cartesian_waypoints_planner_B.flag =

                  (cartesian_waypoints_planner_B.b_gamma < obj->StepTolerance);

                if (cartesian_waypoints_planner_B.flag) {

                  for (cartesian_waypoints_planner_B.i_h = 0;

                       cartesian_waypoints_planner_B.i_h < 6;

                       cartesian_waypoints_planner_B.i_h++) {

                    xSol[cartesian_waypoints_planner_B.i_h] =

                      cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.i_h];

                  }



                  *exitFlag = StepSizeBelowMinimum;

                  args = obj->ExtraArgs;

                  for (cartesian_waypoints_planner_B.b_i_h = 0;

                       cartesian_waypoints_planner_B.b_i_h < 36;

                       cartesian_waypoints_planner_B.b_i_h++) {

                    cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]

                      = args->WeightMatrix[cartesian_waypoints_planner_B.b_i_h];

                  }



                  cartesian_waypoints_planner_B.b_i_h = grad->size[0];

                  grad->size[0] = args->ErrTemp->size[0];

                  cartes_emxEnsureCapacity_real_T(grad,

                    cartesian_waypoints_planner_B.b_i_h);

                  cartesian_waypoints_planner_B.i_h = args->ErrTemp->size[0];

                  for (cartesian_waypoints_planner_B.b_i_h = 0;

                       cartesian_waypoints_planner_B.b_i_h <

                       cartesian_waypoints_planner_B.i_h;

                       cartesian_waypoints_planner_B.b_i_h++) {

                    grad->data[cartesian_waypoints_planner_B.b_i_h] =

                      args->ErrTemp->data[cartesian_waypoints_planner_B.b_i_h];

                  }



                  for (cartesian_waypoints_planner_B.b_i_h = 0;

                       cartesian_waypoints_planner_B.b_i_h < 6;

                       cartesian_waypoints_planner_B.b_i_h++) {

                    cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]

                      = 0.0;

                    for (cartesian_waypoints_planner_B.idx = 0;

                         cartesian_waypoints_planner_B.idx < 6;

                         cartesian_waypoints_planner_B.idx++) {

                      cartesian_waypoints_planner_B.A_d =

                        cartesian_waypoints_planner_B.unusedU0[6 *

                        cartesian_waypoints_planner_B.idx +

                        cartesian_waypoints_planner_B.b_i_h] * grad->

                        data[cartesian_waypoints_planner_B.idx] +

                        cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];

                      cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]

                        = cartesian_waypoints_planner_B.A_d;

                    }

                  }



                  *err = cartesian_waypoints_plan_norm_e

                    (cartesian_waypoints_planner_B.x);

                  *iter = static_cast<real_T>

                    (cartesian_waypoints_planner_B.g_idx_0) + 1.0;

                  exitg1 = 1;

                } else {

                  cartesian_waypoints_planner_B.b_gamma *=

                    cartesian_waypoints_planner_B.beta;

                  cartesian_waypoints_planner_B.m++;

                  for (cartesian_waypoints_planner_B.b_i_h = 0;

                       cartesian_waypoints_planner_B.b_i_h < 6;

                       cartesian_waypoints_planner_B.b_i_h++) {

                    cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]

                      = cartesian_waypoints_planner_B.b_gamma *

                      cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]

                      + cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];

                  }



                  cartesian_IKHelpers_computeCost

                    (cartesian_waypoints_planner_B.sNew_a, obj->ExtraArgs,

                     &cartesian_waypoints_planner_B.costNew,

                     cartesian_waypoints_planner_B.V, unusedU1, &d);

                  obj->ExtraArgs = d;

                }

              } else {

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h < 6;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  xSol[cartesian_waypoints_planner_B.b_i_h] +=

                    cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];

                }



                args = obj->ExtraArgs;

                cartesian_waypoints_planner_B.b_i_h = alpha->size[0];

                alpha->size[0] = args->GradTemp->size[0];

                cartes_emxEnsureCapacity_real_T(alpha,

                  cartesian_waypoints_planner_B.b_i_h);

                cartesian_waypoints_planner_B.i_h = args->GradTemp->size[0];

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h <

                     cartesian_waypoints_planner_B.i_h;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  alpha->data[cartesian_waypoints_planner_B.b_i_h] =

                    args->GradTemp->data[cartesian_waypoints_planner_B.b_i_h];

                }



                exitg1 = 2;

              }

            } while (exitg1 == 0);



            if (exitg1 == 1) {

              exitg2 = 1;

            } else if ((cartesian_waypoints_planner_B.m == 0.0) && (fabs

                        (cartesian_waypoints_planner_B.b_gamma -

                         cartesian_waypoints_planner_B.s_i) <

                        1.4901161193847656E-8)) {

              cartesian_waypoints_planner_B.i_h = obj->ConstraintMatrix->size[0];

              cartesian_waypoints_planner_B.b_i_h = grad->size[0];

              grad->size[0] = cartesian_waypoints_planner_B.i_h;

              cartes_emxEnsureCapacity_real_T(grad,

                cartesian_waypoints_planner_B.b_i_h);

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <

                   cartesian_waypoints_planner_B.i_h;

                   cartesian_waypoints_planner_B.b_i_h++) {

                grad->data[cartesian_waypoints_planner_B.b_i_h] =

                  obj->ConstraintMatrix->data

                  [(cartesian_waypoints_planner_B.idxl - 1) *

                  obj->ConstraintMatrix->size[0] +

                  cartesian_waypoints_planner_B.b_i_h];

              }



              activeSet->data[cartesian_waypoints_planner_B.idxl - 1] = true;

              cartesian_waypoints_planner_B.nx_f = activeSet->size[0] - 1;

              cartesian_waypoints_planner_B.idx = 0;

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <=

                   cartesian_waypoints_planner_B.nx_f;

                   cartesian_waypoints_planner_B.b_i_h++) {

                if (activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {

                  cartesian_waypoints_planner_B.idx++;

                }

              }



              cartesian_waypoints_planner_B.b_i_h = gb->size[0];

              gb->size[0] = cartesian_waypoints_planner_B.idx;

              carte_emxEnsureCapacity_int32_T(gb,

                cartesian_waypoints_planner_B.b_i_h);

              cartesian_waypoints_planner_B.idx = 0;

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <=

                   cartesian_waypoints_planner_B.nx_f;

                   cartesian_waypoints_planner_B.b_i_h++) {

                if (activeSet->data[cartesian_waypoints_planner_B.b_i_h]) {

                  gb->data[cartesian_waypoints_planner_B.idx] =

                    cartesian_waypoints_planner_B.b_i_h + 1;

                  cartesian_waypoints_planner_B.idx++;

                }

              }



              cartesian_waypoints_planner_B.i_h = obj->ConstraintMatrix->size[0];

              cartesian_waypoints_planner_B.b_i_h = A->size[0] * A->size[1];

              A->size[0] = cartesian_waypoints_planner_B.i_h;

              A->size[1] = gb->size[0];

              cartes_emxEnsureCapacity_real_T(A,

                cartesian_waypoints_planner_B.b_i_h);

              cartesian_waypoints_planner_B.n_n = gb->size[0];

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <

                   cartesian_waypoints_planner_B.n_n;

                   cartesian_waypoints_planner_B.b_i_h++) {

                for (cartesian_waypoints_planner_B.idx = 0;

                     cartesian_waypoints_planner_B.idx <

                     cartesian_waypoints_planner_B.i_h;

                     cartesian_waypoints_planner_B.idx++) {

                  A->data[cartesian_waypoints_planner_B.idx + A->size[0] *

                    cartesian_waypoints_planner_B.b_i_h] = obj->

                    ConstraintMatrix->data[(gb->

                    data[cartesian_waypoints_planner_B.b_i_h] - 1) *

                    obj->ConstraintMatrix->size[0] +

                    cartesian_waypoints_planner_B.idx];

                }

              }



              cartesian_waypoints_planner_B.b_i_h = AIn->size[0] * AIn->size[1];

              AIn->size[0] = grad->size[0];

              AIn->size[1] = grad->size[0];

              cartes_emxEnsureCapacity_real_T(AIn,

                cartesian_waypoints_planner_B.b_i_h);

              cartesian_waypoints_planner_B.i_h = grad->size[0];

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <

                   cartesian_waypoints_planner_B.i_h;

                   cartesian_waypoints_planner_B.b_i_h++) {

                cartesian_waypoints_planner_B.n_n = grad->size[0];

                for (cartesian_waypoints_planner_B.idx = 0;

                     cartesian_waypoints_planner_B.idx <

                     cartesian_waypoints_planner_B.n_n;

                     cartesian_waypoints_planner_B.idx++) {

                  AIn->data[cartesian_waypoints_planner_B.idx + AIn->size[0] *

                    cartesian_waypoints_planner_B.b_i_h] = grad->

                    data[cartesian_waypoints_planner_B.idx] * grad->

                    data[cartesian_waypoints_planner_B.b_i_h];

                }

              }



              cartesian_waypoints_planner_B.m_ew = AIn->size[0] - 1;

              cartesian_waypoints_planner_B.inner = AIn->size[1] - 1;

              cartesian_waypoints_planner_B.b_i_h = y_0->size[0] * y_0->size[1];

              y_0->size[0] = AIn->size[0];

              y_0->size[1] = 6;

              cartes_emxEnsureCapacity_real_T(y_0,

                cartesian_waypoints_planner_B.b_i_h);

              for (cartesian_waypoints_planner_B.idx = 0;

                   cartesian_waypoints_planner_B.idx < 6;

                   cartesian_waypoints_planner_B.idx++) {

                cartesian_waypoints_planner_B.coffset =

                  (cartesian_waypoints_planner_B.m_ew + 1) *

                  cartesian_waypoints_planner_B.idx - 1;

                cartesian_waypoints_planner_B.boffset =

                  cartesian_waypoints_planner_B.idx * 6 - 1;

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h <=

                     cartesian_waypoints_planner_B.m_ew;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  y_0->data[(cartesian_waypoints_planner_B.coffset +

                             cartesian_waypoints_planner_B.b_i_h) + 1] = 0.0;

                }



                for (cartesian_waypoints_planner_B.nx_f = 0;

                     cartesian_waypoints_planner_B.nx_f <=

                     cartesian_waypoints_planner_B.inner;

                     cartesian_waypoints_planner_B.nx_f++) {

                  cartesian_waypoints_planner_B.aoffset =

                    cartesian_waypoints_planner_B.nx_f * AIn->size[0] - 1;

                  cartesian_waypoints_planner_B.s_i =

                    cartesian_waypoints_planner_B.H

                    [(cartesian_waypoints_planner_B.boffset +

                      cartesian_waypoints_planner_B.nx_f) + 1];

                  for (cartesian_waypoints_planner_B.j_p = 0;

                       cartesian_waypoints_planner_B.j_p <=

                       cartesian_waypoints_planner_B.m_ew;

                       cartesian_waypoints_planner_B.j_p++) {

                    cartesian_waypoints_planner_B.i_h =

                      cartesian_waypoints_planner_B.j_p + 1;

                    cartesian_waypoints_planner_B.b_i_h =

                      cartesian_waypoints_planner_B.coffset +

                      cartesian_waypoints_planner_B.i_h;

                    y_0->data[cartesian_waypoints_planner_B.b_i_h] += AIn->

                      data[cartesian_waypoints_planner_B.aoffset +

                      cartesian_waypoints_planner_B.i_h] *

                      cartesian_waypoints_planner_B.s_i;

                  }

                }

              }



              cartesian_waypoints_planner_B.b_i_h = grad_1->size[0] *

                grad_1->size[1];

              grad_1->size[0] = 1;

              grad_1->size[1] = grad->size[0];

              cartes_emxEnsureCapacity_real_T(grad_1,

                cartesian_waypoints_planner_B.b_i_h);

              cartesian_waypoints_planner_B.i_h = grad->size[0];

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <

                   cartesian_waypoints_planner_B.i_h;

                   cartesian_waypoints_planner_B.b_i_h++) {

                grad_1->data[cartesian_waypoints_planner_B.b_i_h] = grad->

                  data[cartesian_waypoints_planner_B.b_i_h];

              }



              cartesian_waypoints_planner_B.beta = 0.0;

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h < 6;

                   cartesian_waypoints_planner_B.b_i_h++) {

                cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]

                  = 0.0;

                for (cartesian_waypoints_planner_B.idx = 0;

                     cartesian_waypoints_planner_B.idx < 6;

                     cartesian_waypoints_planner_B.idx++) {

                  cartesian_waypoints_planner_B.sigma =

                    cartesian_waypoints_planner_B.H[6 *

                    cartesian_waypoints_planner_B.b_i_h +

                    cartesian_waypoints_planner_B.idx] * grad_1->

                    data[cartesian_waypoints_planner_B.idx] +

                    cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h];

                  cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]

                    = cartesian_waypoints_planner_B.sigma;

                }



                cartesian_waypoints_planner_B.beta +=

                  cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]

                  * grad->data[cartesian_waypoints_planner_B.b_i_h];

              }



              cartesian_waypoints_planner_B.s_i = 1.0 /

                cartesian_waypoints_planner_B.beta;

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h < 6;

                   cartesian_waypoints_planner_B.b_i_h++) {

                for (cartesian_waypoints_planner_B.idx = 0;

                     cartesian_waypoints_planner_B.idx < 6;

                     cartesian_waypoints_planner_B.idx++) {

                  cartesian_waypoints_planner_B.idxl =

                    cartesian_waypoints_planner_B.b_i_h + 6 *

                    cartesian_waypoints_planner_B.idx;

                  cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.idxl]

                    = 0.0;

                  for (cartesian_waypoints_planner_B.i_h = 0;

                       cartesian_waypoints_planner_B.i_h < 6;

                       cartesian_waypoints_planner_B.i_h++) {

                    cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.idxl]

                      += cartesian_waypoints_planner_B.H[6 *

                      cartesian_waypoints_planner_B.i_h +

                      cartesian_waypoints_planner_B.b_i_h] * y_0->data[6 *

                      cartesian_waypoints_planner_B.idx +

                      cartesian_waypoints_planner_B.i_h];

                  }

                }

              }



              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h < 36;

                   cartesian_waypoints_planner_B.b_i_h++) {

                cartesian_waypoints_planner_B.H[cartesian_waypoints_planner_B.b_i_h]

                  -= cartesian_waypoints_planner_B.s_i *

                  cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.b_i_h];

              }



              guard1 = true;

            } else {

              cartesian_waypoints_planner_B.b_i_h = grad->size[0];

              grad->size[0] = alpha->size[0];

              cartes_emxEnsureCapacity_real_T(grad,

                cartesian_waypoints_planner_B.b_i_h);

              cartesian_waypoints_planner_B.i_h = alpha->size[0];

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <

                   cartesian_waypoints_planner_B.i_h;

                   cartesian_waypoints_planner_B.b_i_h++) {

                grad->data[cartesian_waypoints_planner_B.b_i_h] = alpha->

                  data[cartesian_waypoints_planner_B.b_i_h] - grad->

                  data[cartesian_waypoints_planner_B.b_i_h];

              }



              cartesian_waypoints_planner_B.b_gamma = 0.0;

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h < 6;

                   cartesian_waypoints_planner_B.b_i_h++) {

                cartesian_waypoints_planner_B.b_gamma +=

                  cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]

                  * grad->data[cartesian_waypoints_planner_B.b_i_h];

              }



              cartesian_waypoints_planner_B.b_i_h = tmp->size[0] * tmp->size[1];

              tmp->size[0] = 1;

              tmp->size[1] = grad->size[0];

              cartes_emxEnsureCapacity_real_T(tmp,

                cartesian_waypoints_planner_B.b_i_h);

              cartesian_waypoints_planner_B.i_h = grad->size[0];

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <

                   cartesian_waypoints_planner_B.i_h;

                   cartesian_waypoints_planner_B.b_i_h++) {

                tmp->data[cartesian_waypoints_planner_B.b_i_h] = 0.2 *

                  grad->data[cartesian_waypoints_planner_B.b_i_h];

              }



              cartesian_waypoints_planner_B.s_i = 0.0;

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h < 6;

                   cartesian_waypoints_planner_B.b_i_h++) {

                cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]

                  = 0.0;

                for (cartesian_waypoints_planner_B.idx = 0;

                     cartesian_waypoints_planner_B.idx < 6;

                     cartesian_waypoints_planner_B.idx++) {

                  cartesian_waypoints_planner_B.beta =

                    cartesian_waypoints_planner_B.H[6 *

                    cartesian_waypoints_planner_B.b_i_h +

                    cartesian_waypoints_planner_B.idx] * tmp->

                    data[cartesian_waypoints_planner_B.idx] +

                    cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h];

                  cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]

                    = cartesian_waypoints_planner_B.beta;

                }



                cartesian_waypoints_planner_B.s_i +=

                  cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]

                  * grad->data[cartesian_waypoints_planner_B.b_i_h];

              }



              if (cartesian_waypoints_planner_B.b_gamma <

                  cartesian_waypoints_planner_B.s_i) {

                cartesian_waypoints_planner_B.b_i_h = tmp_0->size[0] *

                  tmp_0->size[1];

                tmp_0->size[0] = 1;

                tmp_0->size[1] = grad->size[0];

                cartes_emxEnsureCapacity_real_T(tmp_0,

                  cartesian_waypoints_planner_B.b_i_h);

                cartesian_waypoints_planner_B.i_h = grad->size[0];

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h <

                     cartesian_waypoints_planner_B.i_h;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  tmp_0->data[cartesian_waypoints_planner_B.b_i_h] = 0.8 *

                    grad->data[cartesian_waypoints_planner_B.b_i_h];

                }



                cartesian_waypoints_planner_B.s_i = 0.0;

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h < 6;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]

                    = 0.0;

                  for (cartesian_waypoints_planner_B.idx = 0;

                       cartesian_waypoints_planner_B.idx < 6;

                       cartesian_waypoints_planner_B.idx++) {

                    cartesian_waypoints_planner_B.beta =

                      cartesian_waypoints_planner_B.H[6 *

                      cartesian_waypoints_planner_B.b_i_h +

                      cartesian_waypoints_planner_B.idx] * tmp_0->

                      data[cartesian_waypoints_planner_B.idx] +

                      cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h];

                    cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]

                      = cartesian_waypoints_planner_B.beta;

                  }



                  cartesian_waypoints_planner_B.s_i +=

                    cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]

                    * grad->data[cartesian_waypoints_planner_B.b_i_h];

                }



                cartesian_waypoints_planner_B.b_i_h = grad_0->size[0] *

                  grad_0->size[1];

                grad_0->size[0] = 1;

                grad_0->size[1] = grad->size[0];

                cartes_emxEnsureCapacity_real_T(grad_0,

                  cartesian_waypoints_planner_B.b_i_h);

                cartesian_waypoints_planner_B.i_h = grad->size[0];

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h <

                     cartesian_waypoints_planner_B.i_h;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  grad_0->data[cartesian_waypoints_planner_B.b_i_h] = grad->

                    data[cartesian_waypoints_planner_B.b_i_h];

                }



                cartesian_waypoints_planner_B.beta = 0.0;

                cartesian_waypoints_planner_B.b_gamma = 0.0;

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h < 6;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]

                    = 0.0;

                  for (cartesian_waypoints_planner_B.idx = 0;

                       cartesian_waypoints_planner_B.idx < 6;

                       cartesian_waypoints_planner_B.idx++) {

                    cartesian_waypoints_planner_B.sigma =

                      cartesian_waypoints_planner_B.H[6 *

                      cartesian_waypoints_planner_B.b_i_h +

                      cartesian_waypoints_planner_B.idx] * grad_0->

                      data[cartesian_waypoints_planner_B.idx] +

                      cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h];

                    cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]

                      = cartesian_waypoints_planner_B.sigma;

                  }



                  cartesian_waypoints_planner_B.beta +=

                    cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]

                    * grad->data[cartesian_waypoints_planner_B.b_i_h];

                  cartesian_waypoints_planner_B.b_gamma +=

                    cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h]

                    * grad->data[cartesian_waypoints_planner_B.b_i_h];

                }



                cartesian_waypoints_planner_B.b_gamma =

                  cartesian_waypoints_planner_B.s_i /

                  (cartesian_waypoints_planner_B.beta -

                   cartesian_waypoints_planner_B.b_gamma);

              } else {

                cartesian_waypoints_planner_B.b_gamma = 1.0;

              }



              cartesian_waypoints_planner_B.beta = 0.0;

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h < 6;

                   cartesian_waypoints_planner_B.b_i_h++) {

                cartesian_waypoints_planner_B.s_i = 0.0;

                for (cartesian_waypoints_planner_B.idx = 0;

                     cartesian_waypoints_planner_B.idx < 6;

                     cartesian_waypoints_planner_B.idx++) {

                  cartesian_waypoints_planner_B.s_i +=

                    cartesian_waypoints_planner_B.H[6 *

                    cartesian_waypoints_planner_B.idx +

                    cartesian_waypoints_planner_B.b_i_h] * (1.0 -

                    cartesian_waypoints_planner_B.b_gamma) * grad->

                    data[cartesian_waypoints_planner_B.idx];

                }



                cartesian_waypoints_planner_B.s_i +=

                  cartesian_waypoints_planner_B.b_gamma *

                  cartesian_waypoints_planner_B.Hg[cartesian_waypoints_planner_B.b_i_h];

                cartesian_waypoints_planner_B.beta +=

                  cartesian_waypoints_planner_B.s_i * grad->

                  data[cartesian_waypoints_planner_B.b_i_h];

                cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]

                  = cartesian_waypoints_planner_B.s_i;

              }



              cartesian_waypoints_planner_B.b_i_h = sNew->size[0] * sNew->size[1];

              sNew->size[0] = 6;

              sNew->size[1] = grad->size[0];

              cartes_emxEnsureCapacity_real_T(sNew,

                cartesian_waypoints_planner_B.b_i_h);

              cartesian_waypoints_planner_B.i_h = grad->size[0];

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <

                   cartesian_waypoints_planner_B.i_h;

                   cartesian_waypoints_planner_B.b_i_h++) {

                for (cartesian_waypoints_planner_B.idx = 0;

                     cartesian_waypoints_planner_B.idx < 6;

                     cartesian_waypoints_planner_B.idx++) {

                  cartesian_waypoints_planner_B.s_i =

                    cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.idx]

                    * grad->data[cartesian_waypoints_planner_B.b_i_h];

                  sNew->data[cartesian_waypoints_planner_B.idx + 6 *

                    cartesian_waypoints_planner_B.b_i_h] =

                    cartesian_waypoints_planner_B.s_i /

                    cartesian_waypoints_planner_B.beta;

                }

              }



              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h < 36;

                   cartesian_waypoints_planner_B.b_i_h++) {

                cartesian_waypoints_planner_B.V[cartesian_waypoints_planner_B.b_i_h]

                  =

                  cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]

                  - sNew->data[cartesian_waypoints_planner_B.b_i_h];

              }



              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h < 6;

                   cartesian_waypoints_planner_B.b_i_h++) {

                for (cartesian_waypoints_planner_B.idx = 0;

                     cartesian_waypoints_planner_B.idx < 6;

                     cartesian_waypoints_planner_B.idx++) {

                  cartesian_waypoints_planner_B.nx_f =

                    cartesian_waypoints_planner_B.b_i_h + 6 *

                    cartesian_waypoints_planner_B.idx;

                  cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.nx_f]

                    = 0.0;

                  for (cartesian_waypoints_planner_B.i_h = 0;

                       cartesian_waypoints_planner_B.i_h < 6;

                       cartesian_waypoints_planner_B.i_h++) {

                    cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.nx_f]

                      += cartesian_waypoints_planner_B.V[6 *

                      cartesian_waypoints_planner_B.i_h +

                      cartesian_waypoints_planner_B.b_i_h] *

                      cartesian_waypoints_planner_B.H[6 *

                      cartesian_waypoints_planner_B.idx +

                      cartesian_waypoints_planner_B.i_h];

                  }

                }

              }



              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h < 6;

                   cartesian_waypoints_planner_B.b_i_h++) {

                for (cartesian_waypoints_planner_B.idx = 0;

                     cartesian_waypoints_planner_B.idx < 6;

                     cartesian_waypoints_planner_B.idx++) {

                  cartesian_waypoints_planner_B.nx_f =

                    cartesian_waypoints_planner_B.b_i_h + 6 *

                    cartesian_waypoints_planner_B.idx;

                  cartesian_waypoints_planner_B.P[cartesian_waypoints_planner_B.nx_f]

                    = 0.0;

                  for (cartesian_waypoints_planner_B.i_h = 0;

                       cartesian_waypoints_planner_B.i_h < 6;

                       cartesian_waypoints_planner_B.i_h++) {

                    cartesian_waypoints_planner_B.P[cartesian_waypoints_planner_B.nx_f]

                      += cartesian_waypoints_planner_B.H_m[6 *

                      cartesian_waypoints_planner_B.i_h +

                      cartesian_waypoints_planner_B.b_i_h] *

                      cartesian_waypoints_planner_B.V[6 *

                      cartesian_waypoints_planner_B.i_h +

                      cartesian_waypoints_planner_B.idx];

                  }

                }

              }



              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h < 6;

                   cartesian_waypoints_planner_B.b_i_h++) {

                for (cartesian_waypoints_planner_B.idx = 0;

                     cartesian_waypoints_planner_B.idx < 6;

                     cartesian_waypoints_planner_B.idx++) {

                  cartesian_waypoints_planner_B.sNew[cartesian_waypoints_planner_B.idx

                    + 6 * cartesian_waypoints_planner_B.b_i_h] =

                    cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.idx]

                    * cartesian_waypoints_planner_B.sNew_a[cartesian_waypoints_planner_B.b_i_h]

                    / cartesian_waypoints_planner_B.beta;

                }

              }



              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h < 36;

                   cartesian_waypoints_planner_B.b_i_h++) {

                cartesian_waypoints_planner_B.s_i =

                  cartesian_waypoints_planner_B.P[cartesian_waypoints_planner_B.b_i_h]

                  + cartesian_waypoints_planner_B.sNew[cartesian_waypoints_planner_B.b_i_h];

                cartesian_waypoints_planner_B.H_m[cartesian_waypoints_planner_B.b_i_h]

                  = 1.4901161193847656E-8 * static_cast<real_T>

                  (tmp_1[cartesian_waypoints_planner_B.b_i_h]) +

                  cartesian_waypoints_planner_B.s_i;

                cartesian_waypoints_planner_B.H[cartesian_waypoints_planner_B.b_i_h]

                  = cartesian_waypoints_planner_B.s_i;

              }



              if (!cartesian_wa_isPositiveDefinite

                  (cartesian_waypoints_planner_B.H_m)) {

                *exitFlag = HessianNotPositiveSemidefinite;

                args = obj->ExtraArgs;

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h < 36;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]

                    = args->WeightMatrix[cartesian_waypoints_planner_B.b_i_h];

                }



                cartesian_waypoints_planner_B.b_i_h = grad->size[0];

                grad->size[0] = args->ErrTemp->size[0];

                cartes_emxEnsureCapacity_real_T(grad,

                  cartesian_waypoints_planner_B.b_i_h);

                cartesian_waypoints_planner_B.i_h = args->ErrTemp->size[0];

                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h <

                     cartesian_waypoints_planner_B.i_h;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  grad->data[cartesian_waypoints_planner_B.b_i_h] =

                    args->ErrTemp->data[cartesian_waypoints_planner_B.b_i_h];

                }



                for (cartesian_waypoints_planner_B.b_i_h = 0;

                     cartesian_waypoints_planner_B.b_i_h < 6;

                     cartesian_waypoints_planner_B.b_i_h++) {

                  cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]

                    = 0.0;

                  for (cartesian_waypoints_planner_B.idx = 0;

                       cartesian_waypoints_planner_B.idx < 6;

                       cartesian_waypoints_planner_B.idx++) {

                    cartesian_waypoints_planner_B.A_d =

                      cartesian_waypoints_planner_B.unusedU0[6 *

                      cartesian_waypoints_planner_B.idx +

                      cartesian_waypoints_planner_B.b_i_h] * grad->

                      data[cartesian_waypoints_planner_B.idx] +

                      cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];

                    cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]

                      = cartesian_waypoints_planner_B.A_d;

                  }

                }



                *err = cartesian_waypoints_plan_norm_e

                  (cartesian_waypoints_planner_B.x);

                *iter = static_cast<real_T>

                  (cartesian_waypoints_planner_B.g_idx_0) + 1.0;

                exitg2 = 1;

              } else {

                guard1 = true;

              }

            }

          }



          if (guard1) {

            if (DampedBFGSwGradientProjection_e(obj, xSol)) {

              for (cartesian_waypoints_planner_B.i_h = 0;

                   cartesian_waypoints_planner_B.i_h < 6;

                   cartesian_waypoints_planner_B.i_h++) {

                xSol[cartesian_waypoints_planner_B.i_h] =

                  cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.i_h];

              }



              *exitFlag = SearchDirectionInvalid;

              args = obj->ExtraArgs;

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h < 36;

                   cartesian_waypoints_planner_B.b_i_h++) {

                cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]

                  = args->WeightMatrix[cartesian_waypoints_planner_B.b_i_h];

              }



              cartesian_waypoints_planner_B.b_i_h = grad->size[0];

              grad->size[0] = args->ErrTemp->size[0];

              cartes_emxEnsureCapacity_real_T(grad,

                cartesian_waypoints_planner_B.b_i_h);

              cartesian_waypoints_planner_B.i_h = args->ErrTemp->size[0];

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <

                   cartesian_waypoints_planner_B.i_h;

                   cartesian_waypoints_planner_B.b_i_h++) {

                grad->data[cartesian_waypoints_planner_B.b_i_h] = args->

                  ErrTemp->data[cartesian_waypoints_planner_B.b_i_h];

              }



              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h < 6;

                   cartesian_waypoints_planner_B.b_i_h++) {

                cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]

                  = 0.0;

                for (cartesian_waypoints_planner_B.idx = 0;

                     cartesian_waypoints_planner_B.idx < 6;

                     cartesian_waypoints_planner_B.idx++) {

                  cartesian_waypoints_planner_B.A_d =

                    cartesian_waypoints_planner_B.unusedU0[6 *

                    cartesian_waypoints_planner_B.idx +

                    cartesian_waypoints_planner_B.b_i_h] * grad->

                    data[cartesian_waypoints_planner_B.idx] +

                    cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];

                  cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h]

                    = cartesian_waypoints_planner_B.A_d;

                }

              }



              *err = cartesian_waypoints_plan_norm_e

                (cartesian_waypoints_planner_B.x);

              *iter = static_cast<real_T>(cartesian_waypoints_planner_B.g_idx_0)

                + 1.0;

              exitg2 = 1;

            } else {

              for (cartesian_waypoints_planner_B.i_h = 0;

                   cartesian_waypoints_planner_B.i_h < 6;

                   cartesian_waypoints_planner_B.i_h++) {

                cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.i_h]

                  = xSol[cartesian_waypoints_planner_B.i_h];

              }



              cartesian_waypoints_planner_B.b_i_h = grad->size[0];

              grad->size[0] = alpha->size[0];

              cartes_emxEnsureCapacity_real_T(grad,

                cartesian_waypoints_planner_B.b_i_h);

              cartesian_waypoints_planner_B.i_h = alpha->size[0];

              for (cartesian_waypoints_planner_B.b_i_h = 0;

                   cartesian_waypoints_planner_B.b_i_h <

                   cartesian_waypoints_planner_B.i_h;

                   cartesian_waypoints_planner_B.b_i_h++) {

                grad->data[cartesian_waypoints_planner_B.b_i_h] = alpha->

                  data[cartesian_waypoints_planner_B.b_i_h];

              }



              cartesian_waypoints_planner_B.cost =

                cartesian_waypoints_planner_B.costNew;

              cartesian_waypoints_planner_B.g_idx_0++;

            }

          }

        }

      }

    } else {

      *exitFlag = IterationLimitExceeded;

      args = obj->ExtraArgs;

      for (cartesian_waypoints_planner_B.b_i_h = 0;

           cartesian_waypoints_planner_B.b_i_h < 36;

           cartesian_waypoints_planner_B.b_i_h++) {

        cartesian_waypoints_planner_B.unusedU0[cartesian_waypoints_planner_B.b_i_h]

          = args->WeightMatrix[cartesian_waypoints_planner_B.b_i_h];

      }



      cartesian_waypoints_planner_B.b_i_h = grad->size[0];

      grad->size[0] = args->ErrTemp->size[0];

      cartes_emxEnsureCapacity_real_T(grad, cartesian_waypoints_planner_B.b_i_h);

      cartesian_waypoints_planner_B.i_h = args->ErrTemp->size[0];

      for (cartesian_waypoints_planner_B.b_i_h = 0;

           cartesian_waypoints_planner_B.b_i_h <

           cartesian_waypoints_planner_B.i_h;

           cartesian_waypoints_planner_B.b_i_h++) {

        grad->data[cartesian_waypoints_planner_B.b_i_h] = args->ErrTemp->

          data[cartesian_waypoints_planner_B.b_i_h];

      }



      for (cartesian_waypoints_planner_B.b_i_h = 0;

           cartesian_waypoints_planner_B.b_i_h < 6;

           cartesian_waypoints_planner_B.b_i_h++) {

        cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h] =

          0.0;

        for (cartesian_waypoints_planner_B.idx = 0;

             cartesian_waypoints_planner_B.idx < 6;

             cartesian_waypoints_planner_B.idx++) {

          cartesian_waypoints_planner_B.A_d =

            cartesian_waypoints_planner_B.unusedU0[6 *

            cartesian_waypoints_planner_B.idx +

            cartesian_waypoints_planner_B.b_i_h] * grad->

            data[cartesian_waypoints_planner_B.idx] +

            cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h];

          cartesian_waypoints_planner_B.x[cartesian_waypoints_planner_B.b_i_h] =

            cartesian_waypoints_planner_B.A_d;

        }

      }



      *err = cartesian_waypoints_plan_norm_e(cartesian_waypoints_planner_B.x);

      *iter = obj->MaxNumIterationInternal;

      exitg2 = 1;

    }

  } while (exitg2 == 0);



  cartesian_waypo_emxFree_int32_T(&ii_3);

  cartesian_waypoi_emxFree_real_T(&alpha_0);

  cartesian_waypoi_emxFree_real_T(&A_3);

  cartesian_waypoi_emxFree_real_T(&grad_1);

  cartesian_waypo_emxFree_int32_T(&ii_2);

  cartesian_waypo_emxFree_int32_T(&ii_1);

  cartesian_waypoi_emxFree_real_T(&sNew);

  cartesian_waypoi_emxFree_real_T(&grad_0);

  cartesian_waypoi_emxFree_real_T(&tmp_0);

  cartesian_waypoi_emxFree_real_T(&tmp);

  cartesian_waypoi_emxFree_real_T(&sigma);

  cartesian_waypoi_emxFree_real_T(&A_2);

  cartesian_way_emxFree_boolean_T(&x);

  cartesian_waypoi_emxFree_real_T(&y_0);

  cartesian_waypo_emxFree_int32_T(&ii_0);

  cartesian_waypoi_emxFree_real_T(&y);

  cartesian_waypo_emxFree_int32_T(&ii);

  cartesian_waypoi_emxFree_real_T(&a);

  cartesian_waypo_emxFree_int32_T(&gb);

  cartesian_waypo_emxFree_int32_T(&fb);

  cartesian_waypo_emxFree_int32_T(&eb);

  cartesian_waypo_emxFree_int32_T(&db);

  cartesian_waypo_emxFree_int32_T(&cb);

  cartesian_waypoi_emxFree_real_T(&L);

  cartesian_waypoi_emxFree_real_T(&AIn);

  cartesian_waypoi_emxFree_real_T(&alpha);

  cartesian_waypoi_emxFree_real_T(&A);

  cartesian_way_emxFree_boolean_T(&activeSet);

  cartesian_waypoi_emxFree_real_T(&grad);

  cartesian_waypoi_emxFree_real_T(&unusedU1);

}



static void cartesian_waypoints_pl_isfinite(const

  emxArray_real_T_cartesian_way_T *x, emxArray_boolean_T_cartesian__T *b)

{

  emxArray_boolean_T_cartesian__T *tmp;

  int32_T loop_ub;

  int32_T i;

  i = b->size[0];

  b->size[0] = x->size[0];

  car_emxEnsureCapacity_boolean_T(b, i);

  loop_ub = x->size[0];

  for (i = 0; i < loop_ub; i++) {

    b->data[i] = rtIsInf(x->data[i]);

  }



  cartesian_way_emxInit_boolean_T(&tmp, 1);

  i = tmp->size[0];

  tmp->size[0] = x->size[0];

  car_emxEnsureCapacity_boolean_T(tmp, i);

  loop_ub = x->size[0];

  for (i = 0; i < loop_ub; i++) {

    tmp->data[i] = rtIsNaN(x->data[i]);

  }



  i = b->size[0];

  car_emxEnsureCapacity_boolean_T(b, i);

  loop_ub = b->size[0];

  for (i = 0; i < loop_ub; i++) {

    b->data[i] = ((!b->data[i]) && (!tmp->data[i]));

  }



  cartesian_way_emxFree_boolean_T(&tmp);

}



static void cartesi_genrand_uint32_vector_e(uint32_T mt[625], uint32_T u[2])

{

  for (cartesian_waypoints_planner_B.b_j_e = 0;

       cartesian_waypoints_planner_B.b_j_e < 2;

       cartesian_waypoints_planner_B.b_j_e++) {

    cartesian_waypoints_planner_B.mti = mt[624] + 1U;

    if (cartesian_waypoints_planner_B.mti >= 625U) {

      for (cartesian_waypoints_planner_B.b_kk = 0;

           cartesian_waypoints_planner_B.b_kk < 227;

           cartesian_waypoints_planner_B.b_kk++) {

        cartesian_waypoints_planner_B.y_m5 =

          (mt[cartesian_waypoints_planner_B.b_kk + 1] & 2147483647U) |

          (mt[cartesian_waypoints_planner_B.b_kk] & 2147483648U);

        if ((cartesian_waypoints_planner_B.y_m5 & 1U) == 0U) {

          cartesian_waypoints_planner_B.y_m5 >>= 1U;

        } else {

          cartesian_waypoints_planner_B.y_m5 =

            cartesian_waypoints_planner_B.y_m5 >> 1U ^ 2567483615U;

        }



        mt[cartesian_waypoints_planner_B.b_kk] =

          mt[cartesian_waypoints_planner_B.b_kk + 397] ^

          cartesian_waypoints_planner_B.y_m5;

      }



      for (cartesian_waypoints_planner_B.b_kk = 0;

           cartesian_waypoints_planner_B.b_kk < 396;

           cartesian_waypoints_planner_B.b_kk++) {

        cartesian_waypoints_planner_B.y_m5 =

          (mt[cartesian_waypoints_planner_B.b_kk + 227] & 2147483648U) |

          (mt[cartesian_waypoints_planner_B.b_kk + 228] & 2147483647U);

        if ((cartesian_waypoints_planner_B.y_m5 & 1U) == 0U) {

          cartesian_waypoints_planner_B.y_m5 >>= 1U;

        } else {

          cartesian_waypoints_planner_B.y_m5 =

            cartesian_waypoints_planner_B.y_m5 >> 1U ^ 2567483615U;

        }



        mt[cartesian_waypoints_planner_B.b_kk + 227] =

          mt[cartesian_waypoints_planner_B.b_kk] ^

          cartesian_waypoints_planner_B.y_m5;

      }



      cartesian_waypoints_planner_B.y_m5 = (mt[623] & 2147483648U) | (mt[0] &

        2147483647U);

      if ((cartesian_waypoints_planner_B.y_m5 & 1U) == 0U) {

        cartesian_waypoints_planner_B.y_m5 >>= 1U;

      } else {

        cartesian_waypoints_planner_B.y_m5 = cartesian_waypoints_planner_B.y_m5 >>

          1U ^ 2567483615U;

      }



      mt[623] = mt[396] ^ cartesian_waypoints_planner_B.y_m5;

      cartesian_waypoints_planner_B.mti = 1U;

    }



    cartesian_waypoints_planner_B.y_m5 = mt[static_cast<int32_T>

      (cartesian_waypoints_planner_B.mti) - 1];

    mt[624] = cartesian_waypoints_planner_B.mti;

    cartesian_waypoints_planner_B.y_m5 ^= cartesian_waypoints_planner_B.y_m5 >>

      11U;

    cartesian_waypoints_planner_B.y_m5 ^= cartesian_waypoints_planner_B.y_m5 <<

      7U & 2636928640U;

    cartesian_waypoints_planner_B.y_m5 ^= cartesian_waypoints_planner_B.y_m5 <<

      15U & 4022730752U;

    u[cartesian_waypoints_planner_B.b_j_e] = cartesian_waypoints_planner_B.y_m5 >>

      18U ^ cartesian_waypoints_planner_B.y_m5;

  }

}



static boolean_T cartesian_waypoi_is_valid_state(const uint32_T mt[625])

{

  boolean_T isvalid;

  boolean_T exitg1;

  if ((mt[624] >= 1U) && (mt[624] < 625U)) {

    isvalid = true;

  } else {

    isvalid = false;

  }



  if (isvalid) {

    isvalid = false;

    cartesian_waypoints_planner_B.k_jc = 0;

    exitg1 = false;

    while ((!exitg1) && (cartesian_waypoints_planner_B.k_jc + 1 < 625)) {

      if (mt[cartesian_waypoints_planner_B.k_jc] == 0U) {

        cartesian_waypoints_planner_B.k_jc++;

      } else {

        isvalid = true;

        exitg1 = true;

      }

    }

  }



  return isvalid;

}



static real_T cartesian_waypoints__genrandu_e(uint32_T mt[625])

{

  real_T r;

  int32_T exitg1;



  // ========================= COPYRIGHT NOTICE ============================

  //  This is a uniform (0,1) pseudorandom number generator based on:

  //

  //  A C-program for MT19937, with initialization improved 2002/1/26.

  //  Coded by Takuji Nishimura and Makoto Matsumoto.

  //

  //  Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura,

  //  All rights reserved.

  //

  //  Redistribution and use in source and binary forms, with or without

  //  modification, are permitted provided that the following conditions

  //  are met:

  //

  //    1. Redistributions of source code must retain the above copyright

  //       notice, this list of conditions and the following disclaimer.

  //

  //    2. Redistributions in binary form must reproduce the above copyright

  //       notice, this list of conditions and the following disclaimer

  //       in the documentation and/or other materials provided with the

  //       distribution.

  //

  //    3. The names of its contributors may not be used to endorse or

  //       promote products derived from this software without specific

  //       prior written permission.

  //

  //  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

  //  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

  //  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR

  //  A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT

  //  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,

  //  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT

  //  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,

  //  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY

  //  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT

  //  (INCLUDING  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE

  //  OF THIS  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

  //

  // =============================   END   =================================

  do {

    exitg1 = 0;

    cartesi_genrand_uint32_vector_e(mt, cartesian_waypoints_planner_B.b_u_d);

    r = (static_cast<real_T>(cartesian_waypoints_planner_B.b_u_d[0] >> 5U) *

         6.7108864E+7 + static_cast<real_T>(cartesian_waypoints_planner_B.b_u_d

          [1] >> 6U)) * 1.1102230246251565E-16;

    if (r == 0.0) {

      if (!cartesian_waypoi_is_valid_state(mt)) {

        cartesian_waypoints_planner_B.r_d = 5489U;

        mt[0] = 5489U;

        for (cartesian_waypoints_planner_B.b_mti_l = 0;

             cartesian_waypoints_planner_B.b_mti_l < 623;

             cartesian_waypoints_planner_B.b_mti_l++) {

          cartesian_waypoints_planner_B.r_d =

            ((cartesian_waypoints_planner_B.r_d >> 30U ^

              cartesian_waypoints_planner_B.r_d) * 1812433253U +

             cartesian_waypoints_planner_B.b_mti_l) + 1U;

          mt[cartesian_waypoints_planner_B.b_mti_l + 1] =

            cartesian_waypoints_planner_B.r_d;

        }



        mt[624] = 624U;

      }

    } else {

      exitg1 = 1;

    }

  } while (exitg1 == 0);



  return r;

}



static real_T cartesia_eml_rand_mt19937ar_evq(uint32_T state[625])

{

  real_T r;

  static const real_T tmp[257] = { 1.0, 0.977101701267673, 0.959879091800108,

    0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131,

    0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857,

    0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884,

    0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964,

    0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187,

    0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157,

    0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863,

    0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512,

    0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774,

    0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697,

    0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624,

    0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914,

    0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987,

    0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354,

    0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766,

    0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952,

    0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322,

    0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468,

    0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458,

    0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773,

    0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716,

    0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804,

    0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022,

    0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188,

    0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832,

    0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393,

    0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032,

    0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077,

    0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522,

    0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601,

    0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337,

    0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803,

    0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183,

    0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668,

    0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261,

    0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942,

    0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501,

    0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681,

    0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331,

    0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881,

    0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135,

    0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268,

    0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839,

    0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366,

    0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263,

    0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514,

    0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669,

    0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597,

    0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839,

    0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487,

    0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009,

    0.093365311912691, 0.0911136480663738, 0.0888735920682759,

    0.0866451944505581, 0.0844285095703535, 0.082223595813203,

    0.0800305158146631, 0.0778493367020961, 0.0756801303589272,

    0.0735229737139814, 0.0713779490588905, 0.0692451443970068,

    0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954,

    0.0587679529209339, 0.0567106901062031, 0.0546664613248891,

    0.0526354182767924, 0.0506177238609479, 0.0486135532158687,

    0.0466230949019305, 0.0446465522512946, 0.0426841449164746,

    0.0407361106559411, 0.0388027074045262, 0.0368842156885674,

    0.0349809414617162, 0.0330932194585786, 0.0312214171919203,

    0.0293659397581334, 0.0275272356696031, 0.0257058040085489,

    0.0239022033057959, 0.0221170627073089, 0.0203510962300445,

    0.0186051212757247, 0.0168800831525432, 0.0151770883079353,

    0.0134974506017399, 0.0118427578579079, 0.0102149714397015,

    0.00861658276939875, 0.00705087547137324, 0.00552240329925101,

    0.00403797259336304, 0.00260907274610216, 0.0012602859304986,

    0.000477467764609386 };



  const real_T *fitab;

  int32_T exitg1;

  cartesian_waypoints_planner_B.xi[0] = 0.0;

  cartesian_waypoints_planner_B.xi[1] = 0.215241895984875;

  cartesian_waypoints_planner_B.xi[2] = 0.286174591792068;

  cartesian_waypoints_planner_B.xi[3] = 0.335737519214422;

  cartesian_waypoints_planner_B.xi[4] = 0.375121332878378;

  cartesian_waypoints_planner_B.xi[5] = 0.408389134611989;

  cartesian_waypoints_planner_B.xi[6] = 0.43751840220787;

  cartesian_waypoints_planner_B.xi[7] = 0.46363433679088;

  cartesian_waypoints_planner_B.xi[8] = 0.487443966139235;

  cartesian_waypoints_planner_B.xi[9] = 0.50942332960209;

  cartesian_waypoints_planner_B.xi[10] = 0.529909720661557;

  cartesian_waypoints_planner_B.xi[11] = 0.549151702327164;

  cartesian_waypoints_planner_B.xi[12] = 0.567338257053817;

  cartesian_waypoints_planner_B.xi[13] = 0.584616766106378;

  cartesian_waypoints_planner_B.xi[14] = 0.601104617755991;

  cartesian_waypoints_planner_B.xi[15] = 0.61689699000775;

  cartesian_waypoints_planner_B.xi[16] = 0.63207223638606;

  cartesian_waypoints_planner_B.xi[17] = 0.646695714894993;

  cartesian_waypoints_planner_B.xi[18] = 0.660822574244419;

  cartesian_waypoints_planner_B.xi[19] = 0.674499822837293;

  cartesian_waypoints_planner_B.xi[20] = 0.687767892795788;

  cartesian_waypoints_planner_B.xi[21] = 0.700661841106814;

  cartesian_waypoints_planner_B.xi[22] = 0.713212285190975;

  cartesian_waypoints_planner_B.xi[23] = 0.725446140909999;

  cartesian_waypoints_planner_B.xi[24] = 0.737387211434295;

  cartesian_waypoints_planner_B.xi[25] = 0.749056662017815;

  cartesian_waypoints_planner_B.xi[26] = 0.760473406430107;

  cartesian_waypoints_planner_B.xi[27] = 0.771654424224568;

  cartesian_waypoints_planner_B.xi[28] = 0.782615023307232;

  cartesian_waypoints_planner_B.xi[29] = 0.793369058840623;

  cartesian_waypoints_planner_B.xi[30] = 0.80392911698997;

  cartesian_waypoints_planner_B.xi[31] = 0.814306670135215;

  cartesian_waypoints_planner_B.xi[32] = 0.824512208752291;

  cartesian_waypoints_planner_B.xi[33] = 0.834555354086381;

  cartesian_waypoints_planner_B.xi[34] = 0.844444954909153;

  cartesian_waypoints_planner_B.xi[35] = 0.854189171008163;

  cartesian_waypoints_planner_B.xi[36] = 0.863795545553308;

  cartesian_waypoints_planner_B.xi[37] = 0.87327106808886;

  cartesian_waypoints_planner_B.xi[38] = 0.882622229585165;

  cartesian_waypoints_planner_B.xi[39] = 0.891855070732941;

  cartesian_waypoints_planner_B.xi[40] = 0.900975224461221;

  cartesian_waypoints_planner_B.xi[41] = 0.909987953496718;

  cartesian_waypoints_planner_B.xi[42] = 0.91889818364959;

  cartesian_waypoints_planner_B.xi[43] = 0.927710533401999;

  cartesian_waypoints_planner_B.xi[44] = 0.936429340286575;

  cartesian_waypoints_planner_B.xi[45] = 0.945058684468165;

  cartesian_waypoints_planner_B.xi[46] = 0.953602409881086;

  cartesian_waypoints_planner_B.xi[47] = 0.96206414322304;

  cartesian_waypoints_planner_B.xi[48] = 0.970447311064224;

  cartesian_waypoints_planner_B.xi[49] = 0.978755155294224;

  cartesian_waypoints_planner_B.xi[50] = 0.986990747099062;

  cartesian_waypoints_planner_B.xi[51] = 0.99515699963509;

  cartesian_waypoints_planner_B.xi[52] = 1.00325667954467;

  cartesian_waypoints_planner_B.xi[53] = 1.01129241744;

  cartesian_waypoints_planner_B.xi[54] = 1.01926671746548;

  cartesian_waypoints_planner_B.xi[55] = 1.02718196603564;

  cartesian_waypoints_planner_B.xi[56] = 1.03504043983344;

  cartesian_waypoints_planner_B.xi[57] = 1.04284431314415;

  cartesian_waypoints_planner_B.xi[58] = 1.05059566459093;

  cartesian_waypoints_planner_B.xi[59] = 1.05829648333067;

  cartesian_waypoints_planner_B.xi[60] = 1.06594867476212;

  cartesian_waypoints_planner_B.xi[61] = 1.07355406579244;

  cartesian_waypoints_planner_B.xi[62] = 1.0811144097034;

  cartesian_waypoints_planner_B.xi[63] = 1.08863139065398;

  cartesian_waypoints_planner_B.xi[64] = 1.09610662785202;

  cartesian_waypoints_planner_B.xi[65] = 1.10354167942464;

  cartesian_waypoints_planner_B.xi[66] = 1.11093804601357;

  cartesian_waypoints_planner_B.xi[67] = 1.11829717411934;

  cartesian_waypoints_planner_B.xi[68] = 1.12562045921553;

  cartesian_waypoints_planner_B.xi[69] = 1.13290924865253;

  cartesian_waypoints_planner_B.xi[70] = 1.14016484436815;

  cartesian_waypoints_planner_B.xi[71] = 1.14738850542085;

  cartesian_waypoints_planner_B.xi[72] = 1.15458145035993;

  cartesian_waypoints_planner_B.xi[73] = 1.16174485944561;

  cartesian_waypoints_planner_B.xi[74] = 1.16887987673083;

  cartesian_waypoints_planner_B.xi[75] = 1.17598761201545;

  cartesian_waypoints_planner_B.xi[76] = 1.18306914268269;

  cartesian_waypoints_planner_B.xi[77] = 1.19012551542669;

  cartesian_waypoints_planner_B.xi[78] = 1.19715774787944;

  cartesian_waypoints_planner_B.xi[79] = 1.20416683014438;

  cartesian_waypoints_planner_B.xi[80] = 1.2111537262437;

  cartesian_waypoints_planner_B.xi[81] = 1.21811937548548;

  cartesian_waypoints_planner_B.xi[82] = 1.22506469375653;

  cartesian_waypoints_planner_B.xi[83] = 1.23199057474614;

  cartesian_waypoints_planner_B.xi[84] = 1.23889789110569;

  cartesian_waypoints_planner_B.xi[85] = 1.24578749554863;

  cartesian_waypoints_planner_B.xi[86] = 1.2526602218949;

  cartesian_waypoints_planner_B.xi[87] = 1.25951688606371;

  cartesian_waypoints_planner_B.xi[88] = 1.26635828701823;

  cartesian_waypoints_planner_B.xi[89] = 1.27318520766536;

  cartesian_waypoints_planner_B.xi[90] = 1.27999841571382;

  cartesian_waypoints_planner_B.xi[91] = 1.28679866449324;

  cartesian_waypoints_planner_B.xi[92] = 1.29358669373695;

  cartesian_waypoints_planner_B.xi[93] = 1.30036323033084;

  cartesian_waypoints_planner_B.xi[94] = 1.30712898903073;

  cartesian_waypoints_planner_B.xi[95] = 1.31388467315022;

  cartesian_waypoints_planner_B.xi[96] = 1.32063097522106;

  cartesian_waypoints_planner_B.xi[97] = 1.32736857762793;

  cartesian_waypoints_planner_B.xi[98] = 1.33409815321936;

  cartesian_waypoints_planner_B.xi[99] = 1.3408203658964;

  cartesian_waypoints_planner_B.xi[100] = 1.34753587118059;

  cartesian_waypoints_planner_B.xi[101] = 1.35424531676263;

  cartesian_waypoints_planner_B.xi[102] = 1.36094934303328;

  cartesian_waypoints_planner_B.xi[103] = 1.36764858359748;

  cartesian_waypoints_planner_B.xi[104] = 1.37434366577317;

  cartesian_waypoints_planner_B.xi[105] = 1.38103521107586;

  cartesian_waypoints_planner_B.xi[106] = 1.38772383568998;

  cartesian_waypoints_planner_B.xi[107] = 1.39441015092814;

  cartesian_waypoints_planner_B.xi[108] = 1.40109476367925;

  cartesian_waypoints_planner_B.xi[109] = 1.4077782768464;

  cartesian_waypoints_planner_B.xi[110] = 1.41446128977547;

  cartesian_waypoints_planner_B.xi[111] = 1.42114439867531;

  cartesian_waypoints_planner_B.xi[112] = 1.42782819703026;

  cartesian_waypoints_planner_B.xi[113] = 1.43451327600589;

  cartesian_waypoints_planner_B.xi[114] = 1.44120022484872;

  cartesian_waypoints_planner_B.xi[115] = 1.44788963128058;

  cartesian_waypoints_planner_B.xi[116] = 1.45458208188841;

  cartesian_waypoints_planner_B.xi[117] = 1.46127816251028;

  cartesian_waypoints_planner_B.xi[118] = 1.46797845861808;

  cartesian_waypoints_planner_B.xi[119] = 1.47468355569786;

  cartesian_waypoints_planner_B.xi[120] = 1.48139403962819;

  cartesian_waypoints_planner_B.xi[121] = 1.48811049705745;

  cartesian_waypoints_planner_B.xi[122] = 1.49483351578049;

  cartesian_waypoints_planner_B.xi[123] = 1.50156368511546;

  cartesian_waypoints_planner_B.xi[124] = 1.50830159628131;

  cartesian_waypoints_planner_B.xi[125] = 1.51504784277671;

  cartesian_waypoints_planner_B.xi[126] = 1.521803020761;

  cartesian_waypoints_planner_B.xi[127] = 1.52856772943771;

  cartesian_waypoints_planner_B.xi[128] = 1.53534257144151;

  cartesian_waypoints_planner_B.xi[129] = 1.542128153229;

  cartesian_waypoints_planner_B.xi[130] = 1.54892508547417;

  cartesian_waypoints_planner_B.xi[131] = 1.55573398346918;

  cartesian_waypoints_planner_B.xi[132] = 1.56255546753104;

  cartesian_waypoints_planner_B.xi[133] = 1.56939016341512;

  cartesian_waypoints_planner_B.xi[134] = 1.57623870273591;

  cartesian_waypoints_planner_B.xi[135] = 1.58310172339603;

  cartesian_waypoints_planner_B.xi[136] = 1.58997987002419;

  cartesian_waypoints_planner_B.xi[137] = 1.59687379442279;

  cartesian_waypoints_planner_B.xi[138] = 1.60378415602609;

  cartesian_waypoints_planner_B.xi[139] = 1.61071162236983;

  cartesian_waypoints_planner_B.xi[140] = 1.61765686957301;

  cartesian_waypoints_planner_B.xi[141] = 1.62462058283303;

  cartesian_waypoints_planner_B.xi[142] = 1.63160345693487;

  cartesian_waypoints_planner_B.xi[143] = 1.63860619677555;

  cartesian_waypoints_planner_B.xi[144] = 1.64562951790478;

  cartesian_waypoints_planner_B.xi[145] = 1.65267414708306;

  cartesian_waypoints_planner_B.xi[146] = 1.65974082285818;

  cartesian_waypoints_planner_B.xi[147] = 1.66683029616166;

  cartesian_waypoints_planner_B.xi[148] = 1.67394333092612;

  cartesian_waypoints_planner_B.xi[149] = 1.68108070472517;

  cartesian_waypoints_planner_B.xi[150] = 1.68824320943719;

  cartesian_waypoints_planner_B.xi[151] = 1.69543165193456;

  cartesian_waypoints_planner_B.xi[152] = 1.70264685479992;

  cartesian_waypoints_planner_B.xi[153] = 1.7098896570713;

  cartesian_waypoints_planner_B.xi[154] = 1.71716091501782;

  cartesian_waypoints_planner_B.xi[155] = 1.72446150294804;

  cartesian_waypoints_planner_B.xi[156] = 1.73179231405296;

  cartesian_waypoints_planner_B.xi[157] = 1.73915426128591;

  cartesian_waypoints_planner_B.xi[158] = 1.74654827828172;

  cartesian_waypoints_planner_B.xi[159] = 1.75397532031767;

  cartesian_waypoints_planner_B.xi[160] = 1.76143636531891;

  cartesian_waypoints_planner_B.xi[161] = 1.76893241491127;

  cartesian_waypoints_planner_B.xi[162] = 1.77646449552452;

  cartesian_waypoints_planner_B.xi[163] = 1.78403365954944;

  cartesian_waypoints_planner_B.xi[164] = 1.79164098655216;

  cartesian_waypoints_planner_B.xi[165] = 1.79928758454972;

  cartesian_waypoints_planner_B.xi[166] = 1.80697459135082;

  cartesian_waypoints_planner_B.xi[167] = 1.81470317596628;

  cartesian_waypoints_planner_B.xi[168] = 1.82247454009388;

  cartesian_waypoints_planner_B.xi[169] = 1.83028991968276;

  cartesian_waypoints_planner_B.xi[170] = 1.83815058658281;

  cartesian_waypoints_planner_B.xi[171] = 1.84605785028518;

  cartesian_waypoints_planner_B.xi[172] = 1.8540130597602;

  cartesian_waypoints_planner_B.xi[173] = 1.86201760539967;

  cartesian_waypoints_planner_B.xi[174] = 1.87007292107127;

  cartesian_waypoints_planner_B.xi[175] = 1.878180486293;

  cartesian_waypoints_planner_B.xi[176] = 1.88634182853678;

  cartesian_waypoints_planner_B.xi[177] = 1.8945585256707;

  cartesian_waypoints_planner_B.xi[178] = 1.90283220855043;

  cartesian_waypoints_planner_B.xi[179] = 1.91116456377125;

  cartesian_waypoints_planner_B.xi[180] = 1.91955733659319;

  cartesian_waypoints_planner_B.xi[181] = 1.92801233405266;

  cartesian_waypoints_planner_B.xi[182] = 1.93653142827569;

  cartesian_waypoints_planner_B.xi[183] = 1.94511656000868;

  cartesian_waypoints_planner_B.xi[184] = 1.95376974238465;

  cartesian_waypoints_planner_B.xi[185] = 1.96249306494436;

  cartesian_waypoints_planner_B.xi[186] = 1.97128869793366;

  cartesian_waypoints_planner_B.xi[187] = 1.98015889690048;

  cartesian_waypoints_planner_B.xi[188] = 1.98910600761744;

  cartesian_waypoints_planner_B.xi[189] = 1.99813247135842;

  cartesian_waypoints_planner_B.xi[190] = 2.00724083056053;

  cartesian_waypoints_planner_B.xi[191] = 2.0164337349062;

  cartesian_waypoints_planner_B.xi[192] = 2.02571394786385;

  cartesian_waypoints_planner_B.xi[193] = 2.03508435372962;

  cartesian_waypoints_planner_B.xi[194] = 2.04454796521753;

  cartesian_waypoints_planner_B.xi[195] = 2.05410793165065;

  cartesian_waypoints_planner_B.xi[196] = 2.06376754781173;

  cartesian_waypoints_planner_B.xi[197] = 2.07353026351874;

  cartesian_waypoints_planner_B.xi[198] = 2.0833996939983;

  cartesian_waypoints_planner_B.xi[199] = 2.09337963113879;

  cartesian_waypoints_planner_B.xi[200] = 2.10347405571488;

  cartesian_waypoints_planner_B.xi[201] = 2.11368715068665;

  cartesian_waypoints_planner_B.xi[202] = 2.12402331568952;

  cartesian_waypoints_planner_B.xi[203] = 2.13448718284602;

  cartesian_waypoints_planner_B.xi[204] = 2.14508363404789;

  cartesian_waypoints_planner_B.xi[205] = 2.15581781987674;

  cartesian_waypoints_planner_B.xi[206] = 2.16669518035431;

  cartesian_waypoints_planner_B.xi[207] = 2.17772146774029;

  cartesian_waypoints_planner_B.xi[208] = 2.18890277162636;

  cartesian_waypoints_planner_B.xi[209] = 2.20024554661128;

  cartesian_waypoints_planner_B.xi[210] = 2.21175664288416;

  cartesian_waypoints_planner_B.xi[211] = 2.22344334009251;

  cartesian_waypoints_planner_B.xi[212] = 2.23531338492992;

  cartesian_waypoints_planner_B.xi[213] = 2.24737503294739;

  cartesian_waypoints_planner_B.xi[214] = 2.25963709517379;

  cartesian_waypoints_planner_B.xi[215] = 2.27210899022838;

  cartesian_waypoints_planner_B.xi[216] = 2.28480080272449;

  cartesian_waypoints_planner_B.xi[217] = 2.29772334890286;

  cartesian_waypoints_planner_B.xi[218] = 2.31088825060137;

  cartesian_waypoints_planner_B.xi[219] = 2.32430801887113;

  cartesian_waypoints_planner_B.xi[220] = 2.33799614879653;

  cartesian_waypoints_planner_B.xi[221] = 2.35196722737914;

  cartesian_waypoints_planner_B.xi[222] = 2.36623705671729;

  cartesian_waypoints_planner_B.xi[223] = 2.38082279517208;

  cartesian_waypoints_planner_B.xi[224] = 2.39574311978193;

  cartesian_waypoints_planner_B.xi[225] = 2.41101841390112;

  cartesian_waypoints_planner_B.xi[226] = 2.42667098493715;

  cartesian_waypoints_planner_B.xi[227] = 2.44272531820036;

  cartesian_waypoints_planner_B.xi[228] = 2.4592083743347;

  cartesian_waypoints_planner_B.xi[229] = 2.47614993967052;

  cartesian_waypoints_planner_B.xi[230] = 2.49358304127105;

  cartesian_waypoints_planner_B.xi[231] = 2.51154444162669;

  cartesian_waypoints_planner_B.xi[232] = 2.53007523215985;

  cartesian_waypoints_planner_B.xi[233] = 2.54922155032478;

  cartesian_waypoints_planner_B.xi[234] = 2.56903545268184;

  cartesian_waypoints_planner_B.xi[235] = 2.58957598670829;

  cartesian_waypoints_planner_B.xi[236] = 2.61091051848882;

  cartesian_waypoints_planner_B.xi[237] = 2.63311639363158;

  cartesian_waypoints_planner_B.xi[238] = 2.65628303757674;

  cartesian_waypoints_planner_B.xi[239] = 2.68051464328574;

  cartesian_waypoints_planner_B.xi[240] = 2.70593365612306;

  cartesian_waypoints_planner_B.xi[241] = 2.73268535904401;

  cartesian_waypoints_planner_B.xi[242] = 2.76094400527999;

  cartesian_waypoints_planner_B.xi[243] = 2.79092117400193;

  cartesian_waypoints_planner_B.xi[244] = 2.82287739682644;

  cartesian_waypoints_planner_B.xi[245] = 2.85713873087322;

  cartesian_waypoints_planner_B.xi[246] = 2.89412105361341;

  cartesian_waypoints_planner_B.xi[247] = 2.93436686720889;

  cartesian_waypoints_planner_B.xi[248] = 2.97860327988184;

  cartesian_waypoints_planner_B.xi[249] = 3.02783779176959;

  cartesian_waypoints_planner_B.xi[250] = 3.08352613200214;

  cartesian_waypoints_planner_B.xi[251] = 3.147889289518;

  cartesian_waypoints_planner_B.xi[252] = 3.2245750520478;

  cartesian_waypoints_planner_B.xi[253] = 3.32024473383983;

  cartesian_waypoints_planner_B.xi[254] = 3.44927829856143;

  cartesian_waypoints_planner_B.xi[255] = 3.65415288536101;

  cartesian_waypoints_planner_B.xi[256] = 3.91075795952492;

  fitab = &tmp[0];

  do {

    exitg1 = 0;

    cartesi_genrand_uint32_vector_e(state, cartesian_waypoints_planner_B.u32);

    cartesian_waypoints_planner_B.i_a = static_cast<int32_T>

      ((cartesian_waypoints_planner_B.u32[1] >> 24U) + 1U);

    r = ((static_cast<real_T>(cartesian_waypoints_planner_B.u32[0] >> 3U) *

          1.6777216E+7 + static_cast<real_T>(static_cast<int32_T>

           (cartesian_waypoints_planner_B.u32[1]) & 16777215)) *

         2.2204460492503131E-16 - 1.0) *

      cartesian_waypoints_planner_B.xi[cartesian_waypoints_planner_B.i_a];

    if (fabs(r) <=

        cartesian_waypoints_planner_B.xi[cartesian_waypoints_planner_B.i_a - 1])

    {

      exitg1 = 1;

    } else if (cartesian_waypoints_planner_B.i_a < 256) {

      cartesian_waypoints_planner_B.x_c = cartesian_waypoints__genrandu_e(state);

      if ((fitab[cartesian_waypoints_planner_B.i_a - 1] -

           fitab[cartesian_waypoints_planner_B.i_a]) *

          cartesian_waypoints_planner_B.x_c +

          fitab[cartesian_waypoints_planner_B.i_a] < exp(-0.5 * r * r)) {

        exitg1 = 1;

      }

    } else {

      do {

        cartesian_waypoints_planner_B.x_c = cartesian_waypoints__genrandu_e

          (state);

        cartesian_waypoints_planner_B.x_c = log

          (cartesian_waypoints_planner_B.x_c) * 0.273661237329758;

        cartesian_waypoints_planner_B.d_u = cartesian_waypoints__genrandu_e

          (state);

      } while (!(-2.0 * log(cartesian_waypoints_planner_B.d_u) >

                 cartesian_waypoints_planner_B.x_c *

                 cartesian_waypoints_planner_B.x_c));



      if (r < 0.0) {

        r = cartesian_waypoints_planner_B.x_c - 3.65415288536101;

      } else {

        r = 3.65415288536101 - cartesian_waypoints_planner_B.x_c;

      }



      exitg1 = 1;

    }

  } while (exitg1 == 0);



  return r;

}



static void cartesian_waypoints_plann_randn(const real_T varargin_1[2],

  emxArray_real_T_cartesian_way_T *r)

{

  cartesian_waypoints_planner_B.b_k_c = r->size[0] * r->size[1];

  r->size[0] = static_cast<int32_T>(varargin_1[0]);

  r->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(r, cartesian_waypoints_planner_B.b_k_c);

  cartesian_waypoints_planner_B.d_h = r->size[0] - 1;

  for (cartesian_waypoints_planner_B.b_k_c = 0;

       cartesian_waypoints_planner_B.b_k_c <= cartesian_waypoints_planner_B.d_h;

       cartesian_waypoints_planner_B.b_k_c++) {

    r->data[cartesian_waypoints_planner_B.b_k_c] =

      cartesia_eml_rand_mt19937ar_evq(cartesian_waypoints_planner_DW.state_m);

  }

}



static void cartesian__eml_rand_mt19937ar_e(const uint32_T state[625], uint32_T

  b_state[625], real_T *r)

{

  int32_T exitg1;

  memcpy(&b_state[0], &state[0], 625U * sizeof(uint32_T));



  // ========================= COPYRIGHT NOTICE ============================

  //  This is a uniform (0,1) pseudorandom number generator based on:

  //

  //  A C-program for MT19937, with initialization improved 2002/1/26.

  //  Coded by Takuji Nishimura and Makoto Matsumoto.

  //

  //  Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura,

  //  All rights reserved.

  //

  //  Redistribution and use in source and binary forms, with or without

  //  modification, are permitted provided that the following conditions

  //  are met:

  //

  //    1. Redistributions of source code must retain the above copyright

  //       notice, this list of conditions and the following disclaimer.

  //

  //    2. Redistributions in binary form must reproduce the above copyright

  //       notice, this list of conditions and the following disclaimer

  //       in the documentation and/or other materials provided with the

  //       distribution.

  //

  //    3. The names of its contributors may not be used to endorse or

  //       promote products derived from this software without specific

  //       prior written permission.

  //

  //  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

  //  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

  //  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR

  //  A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT

  //  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,

  //  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT

  //  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,

  //  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY

  //  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT

  //  (INCLUDING  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE

  //  OF THIS  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

  //

  // =============================   END   =================================

  do {

    exitg1 = 0;

    cartesi_genrand_uint32_vector_e(b_state, cartesian_waypoints_planner_B.b_u);

    *r = (static_cast<real_T>(cartesian_waypoints_planner_B.b_u[0] >> 5U) *

          6.7108864E+7 + static_cast<real_T>(cartesian_waypoints_planner_B.b_u[1]

           >> 6U)) * 1.1102230246251565E-16;

    if (*r == 0.0) {

      if (!cartesian_waypoi_is_valid_state(b_state)) {

        cartesian_waypoints_planner_B.r = 5489U;

        b_state[0] = 5489U;

        for (cartesian_waypoints_planner_B.b_mti = 0;

             cartesian_waypoints_planner_B.b_mti < 623;

             cartesian_waypoints_planner_B.b_mti++) {

          cartesian_waypoints_planner_B.r = ((cartesian_waypoints_planner_B.r >>

            30U ^ cartesian_waypoints_planner_B.r) * 1812433253U +

            cartesian_waypoints_planner_B.b_mti) + 1U;

          b_state[cartesian_waypoints_planner_B.b_mti + 1] =

            cartesian_waypoints_planner_B.r;

        }



        b_state[624] = 624U;

      }

    } else {

      exitg1 = 1;

    }

  } while (exitg1 == 0);

}



static void cartesian_waypoints_plan_rand_e(real_T varargin_1,

  emxArray_real_T_cartesian_way_T *r)

{

  cartesian_waypoints_planner_B.b_k_jb = r->size[0];

  cartesian_waypoints_planner_B.d_j = static_cast<int32_T>(varargin_1);

  r->size[0] = cartesian_waypoints_planner_B.d_j;

  cartes_emxEnsureCapacity_real_T(r, cartesian_waypoints_planner_B.b_k_jb);

  cartesian_waypoints_planner_B.d_j--;

  for (cartesian_waypoints_planner_B.b_k_jb = 0;

       cartesian_waypoints_planner_B.b_k_jb <= cartesian_waypoints_planner_B.d_j;

       cartesian_waypoints_planner_B.b_k_jb++) {

    memcpy(&cartesian_waypoints_planner_B.uv1[0],

           &cartesian_waypoints_planner_DW.state_m[0], 625U * sizeof(uint32_T));

    cartesian__eml_rand_mt19937ar_e(cartesian_waypoints_planner_B.uv1,

      cartesian_waypoints_planner_DW.state_m, &r->

      data[cartesian_waypoints_planner_B.b_k_jb]);

  }

}



static void cartes_NLPSolverInterface_solve(h_robotics_core_internal_Damp_T *obj,

  const real_T seed[6], real_T xSol[6], real_T *solutionInfo_Iterations, real_T *

  solutionInfo_RRAttempts, real_T *solutionInfo_Error, real_T

  *solutionInfo_ExitFlag, char_T solutionInfo_Status_data[], int32_T

  solutionInfo_Status_size[2])

{

  emxArray_real_T_cartesian_way_T *newseed;

  f_robotics_manip_internal_IKE_T *args;

  x_robotics_manip_internal_Rig_T *obj_0;

  emxArray_real_T_cartesian_way_T *qi;

  c_rigidBodyJoint_cartesian_wa_T *obj_1;

  emxArray_real_T_cartesian_way_T *ub;

  emxArray_real_T_cartesian_way_T *lb;

  emxArray_real_T_cartesian_way_T *rn;

  emxArray_real_T_cartesian_way_T *e;

  emxArray_boolean_T_cartesian__T *x;

  emxArray_boolean_T_cartesian__T *x_tmp;

  emxArray_boolean_T_cartesian__T *x_tmp_0;

  emxArray_boolean_T_cartesian__T *x_0;

  static const char_T tmp[14] = { 'b', 'e', 's', 't', ' ', 'a', 'v', 'a', 'i',

    'l', 'a', 'b', 'l', 'e' };



  static const char_T tmp_0[7] = { 's', 'u', 'c', 'c', 'e', 's', 's' };



  boolean_T guard1 = false;

  boolean_T guard2 = false;

  boolean_T guard3 = false;

  boolean_T exitg1;

  boolean_T exitg2;

  obj->MaxNumIterationInternal = obj->MaxNumIteration;

  obj->MaxTimeInternal = obj->MaxTime;

  for (cartesian_waypoints_planner_B.i_i = 0; cartesian_waypoints_planner_B.i_i <

       6; cartesian_waypoints_planner_B.i_i++) {

    obj->SeedInternal[cartesian_waypoints_planner_B.i_i] =

      seed[cartesian_waypoints_planner_B.i_i];

  }



  cartesian_waypoints_planner_B.tol = obj->SolutionTolerance;

  obj->TimeObj.StartTime = ctimefun();

  DampedBFGSwGradientProjection_s(obj, xSol,

    &cartesian_waypoints_planner_B.exitFlag, &cartesian_waypoints_planner_B.err,

    &cartesian_waypoints_planner_B.iter);

  *solutionInfo_RRAttempts = 0.0;

  *solutionInfo_Iterations = cartesian_waypoints_planner_B.iter;

  *solutionInfo_Error = cartesian_waypoints_planner_B.err;

  cartesian_waypoints_planner_B.exitFlagPrev =

    cartesian_waypoints_planner_B.exitFlag;

  cartesian_waypoi_emxInit_real_T(&newseed, 1);

  cartesian_waypoi_emxInit_real_T(&qi, 2);

  cartesian_waypoi_emxInit_real_T(&ub, 1);

  cartesian_waypoi_emxInit_real_T(&lb, 1);

  cartesian_waypoi_emxInit_real_T(&rn, 1);

  cartesian_waypoi_emxInit_real_T(&e, 2);

  cartesian_way_emxInit_boolean_T(&x, 1);

  cartesian_way_emxInit_boolean_T(&x_tmp, 1);

  cartesian_way_emxInit_boolean_T(&x_tmp_0, 1);

  cartesian_way_emxInit_boolean_T(&x_0, 1);

  exitg1 = false;

  while ((!exitg1) && (obj->RandomRestart && (cartesian_waypoints_planner_B.err >

           cartesian_waypoints_planner_B.tol))) {

    obj->MaxNumIterationInternal -= cartesian_waypoints_planner_B.iter;

    cartesian_waypoints_planner_B.err = ctimefun();

    cartesian_waypoints_planner_B.err -= obj->TimeObj.StartTime;

    obj->MaxTimeInternal = obj->MaxTime - cartesian_waypoints_planner_B.err;

    if (obj->MaxNumIterationInternal <= 0.0) {

      cartesian_waypoints_planner_B.exitFlag = IterationLimitExceeded;

    }



    if ((cartesian_waypoints_planner_B.exitFlag == IterationLimitExceeded) ||

        (cartesian_waypoints_planner_B.exitFlag == TimeLimitExceeded)) {

      cartesian_waypoints_planner_B.exitFlagPrev =

        cartesian_waypoints_planner_B.exitFlag;

      exitg1 = true;

    } else {

      args = obj->ExtraArgs;

      obj_0 = args->Robot;

      cartesian_waypoints_planner_B.ix = newseed->size[0];

      newseed->size[0] = static_cast<int32_T>(obj_0->PositionNumber);

      cartes_emxEnsureCapacity_real_T(newseed, cartesian_waypoints_planner_B.ix);

      cartesian_waypoints_planner_B.nx = static_cast<int32_T>

        (obj_0->PositionNumber);

      for (cartesian_waypoints_planner_B.ix = 0;

           cartesian_waypoints_planner_B.ix < cartesian_waypoints_planner_B.nx;

           cartesian_waypoints_planner_B.ix++) {

        newseed->data[cartesian_waypoints_planner_B.ix] = 0.0;

      }



      cartesian_waypoints_planner_B.err = obj_0->NumBodies;

      cartesian_waypoints_planner_B.c_h = static_cast<int32_T>

        (cartesian_waypoints_planner_B.err) - 1;

      for (cartesian_waypoints_planner_B.i_i = 0;

           cartesian_waypoints_planner_B.i_i <=

           cartesian_waypoints_planner_B.c_h; cartesian_waypoints_planner_B.i_i

           ++) {

        cartesian_waypoints_planner_B.err = obj_0->

          PositionDoFMap[cartesian_waypoints_planner_B.i_i];

        cartesian_waypoints_planner_B.iter = obj_0->

          PositionDoFMap[cartesian_waypoints_planner_B.i_i + 8];

        if (cartesian_waypoints_planner_B.err <=

            cartesian_waypoints_planner_B.iter) {

          obj_1 = obj_0->Bodies[cartesian_waypoints_planner_B.i_i]

            ->JointInternal;

          if (static_cast<int32_T>(obj_1->PositionNumber) == 0) {

            cartesian_waypoints_planner_B.ix = qi->size[0] * qi->size[1];

            qi->size[0] = 1;

            qi->size[1] = 1;

            cartes_emxEnsureCapacity_real_T(qi, cartesian_waypoints_planner_B.ix);

            qi->data[0] = (rtNaN);

          } else {

            cartesian_waypoints_planner_B.nx = obj_1->

              PositionLimitsInternal->size[0];

            cartesian_waypoints_planner_B.ix = ub->size[0];

            ub->size[0] = cartesian_waypoints_planner_B.nx;

            cartes_emxEnsureCapacity_real_T(ub, cartesian_waypoints_planner_B.ix);

            for (cartesian_waypoints_planner_B.ix = 0;

                 cartesian_waypoints_planner_B.ix <

                 cartesian_waypoints_planner_B.nx;

                 cartesian_waypoints_planner_B.ix++) {

              ub->data[cartesian_waypoints_planner_B.ix] =

                obj_1->PositionLimitsInternal->

                data[cartesian_waypoints_planner_B.ix +

                obj_1->PositionLimitsInternal->size[0]];

            }



            cartesian_waypoints_planner_B.nx = obj_1->

              PositionLimitsInternal->size[0];

            cartesian_waypoints_planner_B.ix = lb->size[0];

            lb->size[0] = cartesian_waypoints_planner_B.nx;

            cartes_emxEnsureCapacity_real_T(lb, cartesian_waypoints_planner_B.ix);

            for (cartesian_waypoints_planner_B.ix = 0;

                 cartesian_waypoints_planner_B.ix <

                 cartesian_waypoints_planner_B.nx;

                 cartesian_waypoints_planner_B.ix++) {

              lb->data[cartesian_waypoints_planner_B.ix] =

                obj_1->PositionLimitsInternal->

                data[cartesian_waypoints_planner_B.ix];

            }



            cartesian_waypoints_pl_isfinite(lb, x_tmp);

            cartesian_waypoints_planner_B.y_p = true;

            cartesian_waypoints_planner_B.ix = 0;

            exitg2 = false;

            while ((!exitg2) && (cartesian_waypoints_planner_B.ix + 1 <=

                                 x_tmp->size[0])) {

              if (!x_tmp->data[cartesian_waypoints_planner_B.ix]) {

                cartesian_waypoints_planner_B.y_p = false;

                exitg2 = true;

              } else {

                cartesian_waypoints_planner_B.ix++;

              }

            }



            guard1 = false;

            guard2 = false;

            guard3 = false;

            if (cartesian_waypoints_planner_B.y_p) {

              cartesian_waypoints_pl_isfinite(ub, x);

              cartesian_waypoints_planner_B.y_p = true;

              cartesian_waypoints_planner_B.ix = 0;

              exitg2 = false;

              while ((!exitg2) && (cartesian_waypoints_planner_B.ix + 1 <=

                                   x->size[0])) {

                if (!x->data[cartesian_waypoints_planner_B.ix]) {

                  cartesian_waypoints_planner_B.y_p = false;

                  exitg2 = true;

                } else {

                  cartesian_waypoints_planner_B.ix++;

                }

              }



              if (cartesian_waypoints_planner_B.y_p) {

                cartesian_waypoints_plan_rand_e(obj_1->PositionNumber, rn);

                cartesian_waypoints_planner_B.ix = qi->size[0] * qi->size[1];

                qi->size[0] = lb->size[0];

                qi->size[1] = 1;

                cartes_emxEnsureCapacity_real_T(qi,

                  cartesian_waypoints_planner_B.ix);

                cartesian_waypoints_planner_B.nx = lb->size[0] - 1;

                for (cartesian_waypoints_planner_B.ix = 0;

                     cartesian_waypoints_planner_B.ix <=

                     cartesian_waypoints_planner_B.nx;

                     cartesian_waypoints_planner_B.ix++) {

                  qi->data[cartesian_waypoints_planner_B.ix] = (ub->

                    data[cartesian_waypoints_planner_B.ix] - lb->

                    data[cartesian_waypoints_planner_B.ix]) * rn->

                    data[cartesian_waypoints_planner_B.ix] + lb->

                    data[cartesian_waypoints_planner_B.ix];

                }

              } else {

                guard3 = true;

              }

            } else {

              guard3 = true;

            }



            if (guard3) {

              cartesian_waypoints_planner_B.y_p = true;

              cartesian_waypoints_planner_B.ix = 0;

              exitg2 = false;

              while ((!exitg2) && (cartesian_waypoints_planner_B.ix + 1 <=

                                   x_tmp->size[0])) {

                if (!x_tmp->data[cartesian_waypoints_planner_B.ix]) {

                  cartesian_waypoints_planner_B.y_p = false;

                  exitg2 = true;

                } else {

                  cartesian_waypoints_planner_B.ix++;

                }

              }



              if (cartesian_waypoints_planner_B.y_p) {

                cartesian_waypoints_pl_isfinite(ub, x);

                cartesian_waypoints_planner_B.ix = x_0->size[0];

                x_0->size[0] = x->size[0];

                car_emxEnsureCapacity_boolean_T(x_0,

                  cartesian_waypoints_planner_B.ix);

                cartesian_waypoints_planner_B.nx = x->size[0];

                for (cartesian_waypoints_planner_B.ix = 0;

                     cartesian_waypoints_planner_B.ix <

                     cartesian_waypoints_planner_B.nx;

                     cartesian_waypoints_planner_B.ix++) {

                  x_0->data[cartesian_waypoints_planner_B.ix] = !x->

                    data[cartesian_waypoints_planner_B.ix];

                }



                if (cartesian_waypoints_planner_any(x_0)) {

                  cartesian_waypoints_planner_B.ub[0] = lb->size[0];

                  cartesian_waypoints_planner_B.ub[1] = 1.0;

                  cartesian_waypoints_plann_randn

                    (cartesian_waypoints_planner_B.ub, qi);

                  cartesian_waypoints_planner_B.nx = qi->size[0] - 1;

                  cartesian_waypoints_planner_B.ix = e->size[0] * e->size[1];

                  e->size[0] = qi->size[0];

                  e->size[1] = 1;

                  cartes_emxEnsureCapacity_real_T(e,

                    cartesian_waypoints_planner_B.ix);

                  for (cartesian_waypoints_planner_B.ix = 0;

                       cartesian_waypoints_planner_B.ix <=

                       cartesian_waypoints_planner_B.nx;

                       cartesian_waypoints_planner_B.ix++) {

                    e->data[cartesian_waypoints_planner_B.ix] = fabs(qi->

                      data[cartesian_waypoints_planner_B.ix]);

                  }



                  cartesian_waypoints_planner_B.ix = qi->size[0] * qi->size[1];

                  qi->size[0] = lb->size[0];

                  qi->size[1] = 1;

                  cartes_emxEnsureCapacity_real_T(qi,

                    cartesian_waypoints_planner_B.ix);

                  cartesian_waypoints_planner_B.nx = lb->size[0] - 1;

                  for (cartesian_waypoints_planner_B.ix = 0;

                       cartesian_waypoints_planner_B.ix <=

                       cartesian_waypoints_planner_B.nx;

                       cartesian_waypoints_planner_B.ix++) {

                    qi->data[cartesian_waypoints_planner_B.ix] = lb->

                      data[cartesian_waypoints_planner_B.ix] + e->

                      data[cartesian_waypoints_planner_B.ix];

                  }

                } else {

                  guard2 = true;

                }

              } else {

                guard2 = true;

              }

            }



            if (guard2) {

              cartesian_waypoints_planner_B.ix = x_tmp_0->size[0];

              x_tmp_0->size[0] = x_tmp->size[0];

              car_emxEnsureCapacity_boolean_T(x_tmp_0,

                cartesian_waypoints_planner_B.ix);

              cartesian_waypoints_planner_B.nx = x_tmp->size[0];

              for (cartesian_waypoints_planner_B.ix = 0;

                   cartesian_waypoints_planner_B.ix <

                   cartesian_waypoints_planner_B.nx;

                   cartesian_waypoints_planner_B.ix++) {

                x_tmp_0->data[cartesian_waypoints_planner_B.ix] = !x_tmp->

                  data[cartesian_waypoints_planner_B.ix];

              }



              if (cartesian_waypoints_planner_any(x_tmp_0)) {

                cartesian_waypoints_pl_isfinite(ub, x);

                cartesian_waypoints_planner_B.y_p = true;

                cartesian_waypoints_planner_B.ix = 0;

                exitg2 = false;

                while ((!exitg2) && (cartesian_waypoints_planner_B.ix + 1 <=

                                     x->size[0])) {

                  if (!x->data[cartesian_waypoints_planner_B.ix]) {

                    cartesian_waypoints_planner_B.y_p = false;

                    exitg2 = true;

                  } else {

                    cartesian_waypoints_planner_B.ix++;

                  }

                }



                if (cartesian_waypoints_planner_B.y_p) {

                  cartesian_waypoints_planner_B.ub[0] = ub->size[0];

                  cartesian_waypoints_planner_B.ub[1] = 1.0;

                  cartesian_waypoints_plann_randn

                    (cartesian_waypoints_planner_B.ub, qi);

                  cartesian_waypoints_planner_B.nx = qi->size[0] - 1;

                  cartesian_waypoints_planner_B.ix = e->size[0] * e->size[1];

                  e->size[0] = qi->size[0];

                  e->size[1] = 1;

                  cartes_emxEnsureCapacity_real_T(e,

                    cartesian_waypoints_planner_B.ix);

                  for (cartesian_waypoints_planner_B.ix = 0;

                       cartesian_waypoints_planner_B.ix <=

                       cartesian_waypoints_planner_B.nx;

                       cartesian_waypoints_planner_B.ix++) {

                    e->data[cartesian_waypoints_planner_B.ix] = fabs(qi->

                      data[cartesian_waypoints_planner_B.ix]);

                  }



                  cartesian_waypoints_planner_B.ix = qi->size[0] * qi->size[1];

                  qi->size[0] = ub->size[0];

                  qi->size[1] = 1;

                  cartes_emxEnsureCapacity_real_T(qi,

                    cartesian_waypoints_planner_B.ix);

                  cartesian_waypoints_planner_B.nx = ub->size[0] - 1;

                  for (cartesian_waypoints_planner_B.ix = 0;

                       cartesian_waypoints_planner_B.ix <=

                       cartesian_waypoints_planner_B.nx;

                       cartesian_waypoints_planner_B.ix++) {

                    qi->data[cartesian_waypoints_planner_B.ix] = ub->

                      data[cartesian_waypoints_planner_B.ix] - e->

                      data[cartesian_waypoints_planner_B.ix];

                  }

                } else {

                  guard1 = true;

                }

              } else {

                guard1 = true;

              }

            }



            if (guard1) {

              cartesian_waypoints_planner_B.ub[0] = ub->size[0];

              cartesian_waypoints_planner_B.ub[1] = 1.0;

              cartesian_waypoints_plann_randn(cartesian_waypoints_planner_B.ub,

                qi);

            }

          }



          if (cartesian_waypoints_planner_B.err >

              cartesian_waypoints_planner_B.iter) {

            cartesian_waypoints_planner_B.nx = 0;

            cartesian_waypoints_planner_B.ix = 0;

          } else {

            cartesian_waypoints_planner_B.nx = static_cast<int32_T>

              (cartesian_waypoints_planner_B.err) - 1;

            cartesian_waypoints_planner_B.ix = static_cast<int32_T>

              (cartesian_waypoints_planner_B.iter);

          }



          cartesian_waypoints_planner_B.unnamed_idx_1 =

            cartesian_waypoints_planner_B.ix - cartesian_waypoints_planner_B.nx;

          for (cartesian_waypoints_planner_B.ix = 0;

               cartesian_waypoints_planner_B.ix <

               cartesian_waypoints_planner_B.unnamed_idx_1;

               cartesian_waypoints_planner_B.ix++) {

            newseed->data[cartesian_waypoints_planner_B.nx +

              cartesian_waypoints_planner_B.ix] = qi->

              data[cartesian_waypoints_planner_B.ix];

          }

        }

      }



      for (cartesian_waypoints_planner_B.ix = 0;

           cartesian_waypoints_planner_B.ix < 6;

           cartesian_waypoints_planner_B.ix++) {

        obj->SeedInternal[cartesian_waypoints_planner_B.ix] = newseed->

          data[cartesian_waypoints_planner_B.ix];

      }



      DampedBFGSwGradientProjection_s(obj, cartesian_waypoints_planner_B.c_xSol,

        &cartesian_waypoints_planner_B.exitFlag,

        &cartesian_waypoints_planner_B.err, &cartesian_waypoints_planner_B.iter);

      if (cartesian_waypoints_planner_B.err < *solutionInfo_Error) {

        for (cartesian_waypoints_planner_B.i_i = 0;

             cartesian_waypoints_planner_B.i_i < 6;

             cartesian_waypoints_planner_B.i_i++) {

          xSol[cartesian_waypoints_planner_B.i_i] =

            cartesian_waypoints_planner_B.c_xSol[cartesian_waypoints_planner_B.i_i];

        }



        *solutionInfo_Error = cartesian_waypoints_planner_B.err;

        cartesian_waypoints_planner_B.exitFlagPrev =

          cartesian_waypoints_planner_B.exitFlag;

      }



      (*solutionInfo_RRAttempts)++;

      *solutionInfo_Iterations += cartesian_waypoints_planner_B.iter;

    }

  }



  cartesian_way_emxFree_boolean_T(&x_0);

  cartesian_way_emxFree_boolean_T(&x_tmp_0);

  cartesian_way_emxFree_boolean_T(&x_tmp);

  cartesian_way_emxFree_boolean_T(&x);

  cartesian_waypoi_emxFree_real_T(&e);

  cartesian_waypoi_emxFree_real_T(&rn);

  cartesian_waypoi_emxFree_real_T(&lb);

  cartesian_waypoi_emxFree_real_T(&ub);

  cartesian_waypoi_emxFree_real_T(&qi);

  cartesian_waypoi_emxFree_real_T(&newseed);

  *solutionInfo_ExitFlag = cartesian_waypoints_planner_B.exitFlagPrev;

  if (*solutionInfo_Error < cartesian_waypoints_planner_B.tol) {

    solutionInfo_Status_size[0] = 1;

    solutionInfo_Status_size[1] = 7;

    for (cartesian_waypoints_planner_B.ix = 0; cartesian_waypoints_planner_B.ix <

         7; cartesian_waypoints_planner_B.ix++) {

      solutionInfo_Status_data[cartesian_waypoints_planner_B.ix] =

        tmp_0[cartesian_waypoints_planner_B.ix];

    }

  } else {

    solutionInfo_Status_size[0] = 1;

    solutionInfo_Status_size[1] = 14;

    for (cartesian_waypoints_planner_B.ix = 0; cartesian_waypoints_planner_B.ix <

         14; cartesian_waypoints_planner_B.ix++) {

      solutionInfo_Status_data[cartesian_waypoints_planner_B.ix] =

        tmp[cartesian_waypoints_planner_B.ix];

    }

  }

}



static void cart_inverseKinematics_stepImpl(b_inverseKinematics_cartesian_T *obj,

  const real_T tform[16], const real_T weights[6], const real_T initialGuess[6],

  real_T QSol[6])

{

  emxArray_real_T_cartesian_way_T *bodyIndices;

  emxArray_real_T_cartesian_way_T *positionIndices;

  x_robotics_manip_internal_Rig_T *obj_0;

  emxArray_char_T_cartesian_way_T *endEffectorName;

  w_robotics_manip_internal_Rig_T *body;

  emxArray_real_T_cartesian_way_T *positionMap;

  emxArray_real_T_cartesian_way_T *e;

  emxArray_int32_T_cartesian_wa_T *h;

  emxArray_real_T_cartesian_way_T *y;

  emxArray_char_T_cartesian_way_T *bname;

  emxArray_real_T_cartesian_way_T *bodyIndices_0;

  boolean_T exitg1;

  c_inverseKinematics_setPoseGoal(obj, tform, weights);

  for (cartesian_waypoints_planner_B.i_c = 0; cartesian_waypoints_planner_B.i_c <

       6; cartesian_waypoints_planner_B.i_c++) {

    QSol[cartesian_waypoints_planner_B.i_c] =

      initialGuess[cartesian_waypoints_planner_B.i_c];

  }



  cartesian_waypoi_emxInit_char_T(&endEffectorName, 2);

  RigidBodyTree_validateConfigu_e(obj->RigidBodyTreeInternal, QSol);

  cartes_NLPSolverInterface_solve(obj->Solver, QSol,

    cartesian_waypoints_planner_B.qvSolRaw, &cartesian_waypoints_planner_B.bid,

    &cartesian_waypoints_planner_B.numPositions,

    &cartesian_waypoints_planner_B.ndbl, &cartesian_waypoints_planner_B.apnd,

    cartesian_waypoints_planner_B.sol_Status_data,

    cartesian_waypoints_planner_B.sol_Status_size);

  obj_0 = obj->RigidBodyTreeInternal;

  cartesian_waypoints_planner_B.partialTrueCount = endEffectorName->size[0] *

    endEffectorName->size[1];

  endEffectorName->size[0] = 1;

  endEffectorName->size[1] = obj->Solver->ExtraArgs->BodyName->size[1];

  cartes_emxEnsureCapacity_char_T(endEffectorName,

    cartesian_waypoints_planner_B.partialTrueCount);

  cartesian_waypoints_planner_B.loop_ub = obj->Solver->ExtraArgs->BodyName->

    size[0] * obj->Solver->ExtraArgs->BodyName->size[1] - 1;

  for (cartesian_waypoints_planner_B.partialTrueCount = 0;

       cartesian_waypoints_planner_B.partialTrueCount <=

       cartesian_waypoints_planner_B.loop_ub;

       cartesian_waypoints_planner_B.partialTrueCount++) {

    endEffectorName->data[cartesian_waypoints_planner_B.partialTrueCount] =

      obj->Solver->ExtraArgs->BodyName->

      data[cartesian_waypoints_planner_B.partialTrueCount];

  }



  cartesian_waypoi_emxInit_real_T(&bodyIndices, 1);

  cartesian_waypoints_planner_B.partialTrueCount = bodyIndices->size[0];

  bodyIndices->size[0] = static_cast<int32_T>(obj_0->NumBodies);

  cartes_emxEnsureCapacity_real_T(bodyIndices,

    cartesian_waypoints_planner_B.partialTrueCount);

  cartesian_waypoints_planner_B.loop_ub = static_cast<int32_T>(obj_0->NumBodies);

  for (cartesian_waypoints_planner_B.partialTrueCount = 0;

       cartesian_waypoints_planner_B.partialTrueCount <

       cartesian_waypoints_planner_B.loop_ub;

       cartesian_waypoints_planner_B.partialTrueCount++) {

    bodyIndices->data[cartesian_waypoints_planner_B.partialTrueCount] = 0.0;

  }



  cartesian_waypoi_emxInit_char_T(&bname, 2);

  cartesian_waypoints_planner_B.bid = -1.0;

  cartesian_waypoints_planner_B.partialTrueCount = bname->size[0] * bname->size

    [1];

  bname->size[0] = 1;

  bname->size[1] = obj_0->Base.NameInternal->size[1];

  cartes_emxEnsureCapacity_char_T(bname,

    cartesian_waypoints_planner_B.partialTrueCount);

  cartesian_waypoints_planner_B.loop_ub = obj_0->Base.NameInternal->size[0] *

    obj_0->Base.NameInternal->size[1] - 1;

  for (cartesian_waypoints_planner_B.partialTrueCount = 0;

       cartesian_waypoints_planner_B.partialTrueCount <=

       cartesian_waypoints_planner_B.loop_ub;

       cartesian_waypoints_planner_B.partialTrueCount++) {

    bname->data[cartesian_waypoints_planner_B.partialTrueCount] =

      obj_0->Base.NameInternal->

      data[cartesian_waypoints_planner_B.partialTrueCount];

  }



  if (cartesian_waypoints_plan_strcmp(bname, endEffectorName)) {

    cartesian_waypoints_planner_B.bid = 0.0;

  } else {

    cartesian_waypoints_planner_B.numPositions = obj_0->NumBodies;

    cartesian_waypoints_planner_B.i_c = 0;

    exitg1 = false;

    while ((!exitg1) && (cartesian_waypoints_planner_B.i_c <=

                         static_cast<int32_T>

                         (cartesian_waypoints_planner_B.numPositions) - 1)) {

      body = obj_0->Bodies[cartesian_waypoints_planner_B.i_c];

      cartesian_waypoints_planner_B.partialTrueCount = bname->size[0] *

        bname->size[1];

      bname->size[0] = 1;

      bname->size[1] = body->NameInternal->size[1];

      cartes_emxEnsureCapacity_char_T(bname,

        cartesian_waypoints_planner_B.partialTrueCount);

      cartesian_waypoints_planner_B.loop_ub = body->NameInternal->size[0] *

        body->NameInternal->size[1] - 1;

      for (cartesian_waypoints_planner_B.partialTrueCount = 0;

           cartesian_waypoints_planner_B.partialTrueCount <=

           cartesian_waypoints_planner_B.loop_ub;

           cartesian_waypoints_planner_B.partialTrueCount++) {

        bname->data[cartesian_waypoints_planner_B.partialTrueCount] =

          body->NameInternal->

          data[cartesian_waypoints_planner_B.partialTrueCount];

      }



      if (cartesian_waypoints_plan_strcmp(bname, endEffectorName)) {

        cartesian_waypoints_planner_B.bid = static_cast<real_T>

          (cartesian_waypoints_planner_B.i_c) + 1.0;

        exitg1 = true;

      } else {

        cartesian_waypoints_planner_B.i_c++;

      }

    }

  }



  cartesian_waypoi_emxFree_char_T(&bname);

  cartesian_waypoi_emxFree_char_T(&endEffectorName);

  if (cartesian_waypoints_planner_B.bid == 0.0) {

    cartesian_waypoints_planner_B.partialTrueCount = bodyIndices->size[0];

    bodyIndices->size[0] = 1;

    cartes_emxEnsureCapacity_real_T(bodyIndices,

      cartesian_waypoints_planner_B.partialTrueCount);

    bodyIndices->data[0] = 0.0;

  } else {

    body = obj_0->Bodies[static_cast<int32_T>(cartesian_waypoints_planner_B.bid)

      - 1];

    cartesian_waypoints_planner_B.bid = 1.0;

    while (body->ParentIndex != 0.0) {

      bodyIndices->data[static_cast<int32_T>(cartesian_waypoints_planner_B.bid)

        - 1] = body->Index;

      body = obj_0->Bodies[static_cast<int32_T>(body->ParentIndex) - 1];

      cartesian_waypoints_planner_B.bid++;

    }



    if (1.0 > cartesian_waypoints_planner_B.bid - 1.0) {

      cartesian_waypoints_planner_B.c_k = -1;

    } else {

      cartesian_waypoints_planner_B.c_k = static_cast<int32_T>

        (cartesian_waypoints_planner_B.bid - 1.0) - 1;

    }



    cartesian_waypoi_emxInit_real_T(&bodyIndices_0, 1);

    cartesian_waypoints_planner_B.partialTrueCount = bodyIndices_0->size[0];

    bodyIndices_0->size[0] = cartesian_waypoints_planner_B.c_k + 3;

    cartes_emxEnsureCapacity_real_T(bodyIndices_0,

      cartesian_waypoints_planner_B.partialTrueCount);

    for (cartesian_waypoints_planner_B.partialTrueCount = 0;

         cartesian_waypoints_planner_B.partialTrueCount <=

         cartesian_waypoints_planner_B.c_k;

         cartesian_waypoints_planner_B.partialTrueCount++) {

      bodyIndices_0->data[cartesian_waypoints_planner_B.partialTrueCount] =

        bodyIndices->data[cartesian_waypoints_planner_B.partialTrueCount];

    }



    bodyIndices_0->data[cartesian_waypoints_planner_B.c_k + 1] = body->Index;

    bodyIndices_0->data[cartesian_waypoints_planner_B.c_k + 2] = 0.0;

    cartesian_waypoints_planner_B.partialTrueCount = bodyIndices->size[0];

    bodyIndices->size[0] = bodyIndices_0->size[0];

    cartes_emxEnsureCapacity_real_T(bodyIndices,

      cartesian_waypoints_planner_B.partialTrueCount);

    cartesian_waypoints_planner_B.loop_ub = bodyIndices_0->size[0];

    for (cartesian_waypoints_planner_B.partialTrueCount = 0;

         cartesian_waypoints_planner_B.partialTrueCount <

         cartesian_waypoints_planner_B.loop_ub;

         cartesian_waypoints_planner_B.partialTrueCount++) {

      bodyIndices->data[cartesian_waypoints_planner_B.partialTrueCount] =

        bodyIndices_0->data[cartesian_waypoints_planner_B.partialTrueCount];

    }



    cartesian_waypoi_emxFree_real_T(&bodyIndices_0);

  }



  obj_0 = obj->RigidBodyTreeInternal;

  cartesian_waypoints_planner_B.c_k = bodyIndices->size[0] - 1;

  cartesian_waypoints_planner_B.loop_ub = 0;

  for (cartesian_waypoints_planner_B.i_c = 0; cartesian_waypoints_planner_B.i_c <=

       cartesian_waypoints_planner_B.c_k; cartesian_waypoints_planner_B.i_c++) {

    if (bodyIndices->data[cartesian_waypoints_planner_B.i_c] != 0.0) {

      cartesian_waypoints_planner_B.loop_ub++;

    }

  }



  cartesian_waypo_emxInit_int32_T(&h, 1);

  cartesian_waypoints_planner_B.partialTrueCount = h->size[0];

  h->size[0] = cartesian_waypoints_planner_B.loop_ub;

  carte_emxEnsureCapacity_int32_T(h,

    cartesian_waypoints_planner_B.partialTrueCount);

  cartesian_waypoints_planner_B.partialTrueCount = 0;

  for (cartesian_waypoints_planner_B.i_c = 0; cartesian_waypoints_planner_B.i_c <=

       cartesian_waypoints_planner_B.c_k; cartesian_waypoints_planner_B.i_c++) {

    if (bodyIndices->data[cartesian_waypoints_planner_B.i_c] != 0.0) {

      h->data[cartesian_waypoints_planner_B.partialTrueCount] =

        cartesian_waypoints_planner_B.i_c + 1;

      cartesian_waypoints_planner_B.partialTrueCount++;

    }

  }



  cartesian_waypoi_emxInit_real_T(&positionMap, 2);

  cartesian_waypoints_planner_B.partialTrueCount = positionMap->size[0] *

    positionMap->size[1];

  positionMap->size[0] = h->size[0];

  positionMap->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(positionMap,

    cartesian_waypoints_planner_B.partialTrueCount);

  cartesian_waypoints_planner_B.loop_ub = h->size[0];

  for (cartesian_waypoints_planner_B.partialTrueCount = 0;

       cartesian_waypoints_planner_B.partialTrueCount <

       cartesian_waypoints_planner_B.loop_ub;

       cartesian_waypoints_planner_B.partialTrueCount++) {

    positionMap->data[cartesian_waypoints_planner_B.partialTrueCount] =

      obj_0->PositionDoFMap[static_cast<int32_T>(bodyIndices->data[h->

      data[cartesian_waypoints_planner_B.partialTrueCount] - 1]) - 1];

  }



  cartesian_waypoints_planner_B.loop_ub = h->size[0];

  for (cartesian_waypoints_planner_B.partialTrueCount = 0;

       cartesian_waypoints_planner_B.partialTrueCount <

       cartesian_waypoints_planner_B.loop_ub;

       cartesian_waypoints_planner_B.partialTrueCount++) {

    positionMap->data[cartesian_waypoints_planner_B.partialTrueCount +

      positionMap->size[0]] = obj_0->PositionDoFMap[static_cast<int32_T>

      (bodyIndices->data[h->data[cartesian_waypoints_planner_B.partialTrueCount]

       - 1]) + 7];

  }



  cartesian_waypo_emxFree_int32_T(&h);

  cartesian_waypoi_emxFree_real_T(&bodyIndices);

  cartesian_waypoi_emxInit_real_T(&positionIndices, 2);

  cartesian_waypoints_planner_B.partialTrueCount = positionIndices->size[0] *

    positionIndices->size[1];

  positionIndices->size[0] = 1;

  positionIndices->size[1] = static_cast<int32_T>(obj_0->PositionNumber);

  cartes_emxEnsureCapacity_real_T(positionIndices,

    cartesian_waypoints_planner_B.partialTrueCount);

  cartesian_waypoints_planner_B.loop_ub = static_cast<int32_T>

    (obj_0->PositionNumber) - 1;

  for (cartesian_waypoints_planner_B.partialTrueCount = 0;

       cartesian_waypoints_planner_B.partialTrueCount <=

       cartesian_waypoints_planner_B.loop_ub;

       cartesian_waypoints_planner_B.partialTrueCount++) {

    positionIndices->data[cartesian_waypoints_planner_B.partialTrueCount] = 0.0;

  }



  cartesian_waypoints_planner_B.bid = 0.0;

  cartesian_waypoints_planner_B.c_k = positionMap->size[0] - 1;

  cartesian_waypoi_emxInit_real_T(&e, 2);

  cartesian_waypoi_emxInit_real_T(&y, 2);

  for (cartesian_waypoints_planner_B.i_c = 0; cartesian_waypoints_planner_B.i_c <=

       cartesian_waypoints_planner_B.c_k; cartesian_waypoints_planner_B.i_c++) {

    cartesian_waypoints_planner_B.numPositions = (positionMap->

      data[cartesian_waypoints_planner_B.i_c + positionMap->size[0]] -

      positionMap->data[cartesian_waypoints_planner_B.i_c]) + 1.0;

    if (cartesian_waypoints_planner_B.numPositions > 0.0) {

      if (cartesian_waypoints_planner_B.numPositions < 1.0) {

        y->size[0] = 1;

        y->size[1] = 0;

      } else if (rtIsInf(cartesian_waypoints_planner_B.numPositions) && (1.0 ==

                  cartesian_waypoints_planner_B.numPositions)) {

        cartesian_waypoints_planner_B.partialTrueCount = y->size[0] * y->size[1];

        y->size[0] = 1;

        y->size[1] = 1;

        cartes_emxEnsureCapacity_real_T(y,

          cartesian_waypoints_planner_B.partialTrueCount);

        y->data[0] = (rtNaN);

      } else {

        cartesian_waypoints_planner_B.partialTrueCount = y->size[0] * y->size[1];

        y->size[0] = 1;

        cartesian_waypoints_planner_B.loop_ub = static_cast<int32_T>(floor

          (cartesian_waypoints_planner_B.numPositions - 1.0));

        y->size[1] = cartesian_waypoints_planner_B.loop_ub + 1;

        cartes_emxEnsureCapacity_real_T(y,

          cartesian_waypoints_planner_B.partialTrueCount);

        for (cartesian_waypoints_planner_B.partialTrueCount = 0;

             cartesian_waypoints_planner_B.partialTrueCount <=

             cartesian_waypoints_planner_B.loop_ub;

             cartesian_waypoints_planner_B.partialTrueCount++) {

          y->data[cartesian_waypoints_planner_B.partialTrueCount] = static_cast<

            real_T>(cartesian_waypoints_planner_B.partialTrueCount) + 1.0;

        }

      }



      if (rtIsNaN(positionMap->data[cartesian_waypoints_planner_B.i_c]) ||

          rtIsNaN(positionMap->data[cartesian_waypoints_planner_B.i_c +

                  positionMap->size[0]])) {

        cartesian_waypoints_planner_B.partialTrueCount = e->size[0] * e->size[1];

        e->size[0] = 1;

        e->size[1] = 1;

        cartes_emxEnsureCapacity_real_T(e,

          cartesian_waypoints_planner_B.partialTrueCount);

        e->data[0] = (rtNaN);

      } else if (positionMap->data[cartesian_waypoints_planner_B.i_c +

                 positionMap->size[0]] < positionMap->

                 data[cartesian_waypoints_planner_B.i_c]) {

        e->size[0] = 1;

        e->size[1] = 0;

      } else if ((rtIsInf(positionMap->data[cartesian_waypoints_planner_B.i_c]) ||

                  rtIsInf(positionMap->data[cartesian_waypoints_planner_B.i_c +

                          positionMap->size[0]])) && (positionMap->

                  data[cartesian_waypoints_planner_B.i_c + positionMap->size[0]]

                  == positionMap->data[cartesian_waypoints_planner_B.i_c])) {

        cartesian_waypoints_planner_B.partialTrueCount = e->size[0] * e->size[1];

        e->size[0] = 1;

        e->size[1] = 1;

        cartes_emxEnsureCapacity_real_T(e,

          cartesian_waypoints_planner_B.partialTrueCount);

        e->data[0] = (rtNaN);

      } else if (floor(positionMap->data[cartesian_waypoints_planner_B.i_c]) ==

                 positionMap->data[cartesian_waypoints_planner_B.i_c]) {

        cartesian_waypoints_planner_B.partialTrueCount = e->size[0] * e->size[1];

        e->size[0] = 1;

        e->size[1] = static_cast<int32_T>(floor(positionMap->

          data[cartesian_waypoints_planner_B.i_c + positionMap->size[0]] -

          positionMap->data[cartesian_waypoints_planner_B.i_c])) + 1;

        cartes_emxEnsureCapacity_real_T(e,

          cartesian_waypoints_planner_B.partialTrueCount);

        cartesian_waypoints_planner_B.loop_ub = static_cast<int32_T>(floor

          (positionMap->data[cartesian_waypoints_planner_B.i_c +

           positionMap->size[0]] - positionMap->

           data[cartesian_waypoints_planner_B.i_c]));

        for (cartesian_waypoints_planner_B.partialTrueCount = 0;

             cartesian_waypoints_planner_B.partialTrueCount <=

             cartesian_waypoints_planner_B.loop_ub;

             cartesian_waypoints_planner_B.partialTrueCount++) {

          e->data[cartesian_waypoints_planner_B.partialTrueCount] =

            positionMap->data[cartesian_waypoints_planner_B.i_c] +

            static_cast<real_T>(cartesian_waypoints_planner_B.partialTrueCount);

        }

      } else {

        cartesian_waypoints_planner_B.ndbl = floor((positionMap->

          data[cartesian_waypoints_planner_B.i_c + positionMap->size[0]] -

          positionMap->data[cartesian_waypoints_planner_B.i_c]) + 0.5);

        cartesian_waypoints_planner_B.apnd = positionMap->

          data[cartesian_waypoints_planner_B.i_c] +

          cartesian_waypoints_planner_B.ndbl;

        cartesian_waypoints_planner_B.cdiff = cartesian_waypoints_planner_B.apnd

          - positionMap->data[cartesian_waypoints_planner_B.i_c +

          positionMap->size[0]];

        cartesian_waypoints_planner_B.u0 = fabs(positionMap->

          data[cartesian_waypoints_planner_B.i_c]);

        cartesian_waypoints_planner_B.u1 = fabs(positionMap->

          data[cartesian_waypoints_planner_B.i_c + positionMap->size[0]]);

        if ((cartesian_waypoints_planner_B.u0 > cartesian_waypoints_planner_B.u1)

            || rtIsNaN(cartesian_waypoints_planner_B.u1)) {

          cartesian_waypoints_planner_B.u1 = cartesian_waypoints_planner_B.u0;

        }



        if (fabs(cartesian_waypoints_planner_B.cdiff) < 4.4408920985006262E-16 *

            cartesian_waypoints_planner_B.u1) {

          cartesian_waypoints_planner_B.ndbl++;

          cartesian_waypoints_planner_B.apnd = positionMap->

            data[cartesian_waypoints_planner_B.i_c + positionMap->size[0]];

        } else if (cartesian_waypoints_planner_B.cdiff > 0.0) {

          cartesian_waypoints_planner_B.apnd =

            (cartesian_waypoints_planner_B.ndbl - 1.0) + positionMap->

            data[cartesian_waypoints_planner_B.i_c];

        } else {

          cartesian_waypoints_planner_B.ndbl++;

        }



        if (cartesian_waypoints_planner_B.ndbl >= 0.0) {

          cartesian_waypoints_planner_B.partialTrueCount = static_cast<int32_T>

            (cartesian_waypoints_planner_B.ndbl);

        } else {

          cartesian_waypoints_planner_B.partialTrueCount = 0;

        }



        cartesian_waypoints_planner_B.loop_ub =

          cartesian_waypoints_planner_B.partialTrueCount - 1;

        cartesian_waypoints_planner_B.partialTrueCount = e->size[0] * e->size[1];

        e->size[0] = 1;

        e->size[1] = cartesian_waypoints_planner_B.loop_ub + 1;

        cartes_emxEnsureCapacity_real_T(e,

          cartesian_waypoints_planner_B.partialTrueCount);

        if (cartesian_waypoints_planner_B.loop_ub + 1 > 0) {

          e->data[0] = positionMap->data[cartesian_waypoints_planner_B.i_c];

          if (cartesian_waypoints_planner_B.loop_ub + 1 > 1) {

            e->data[cartesian_waypoints_planner_B.loop_ub] =

              cartesian_waypoints_planner_B.apnd;

            cartesian_waypoints_planner_B.nm1d2 =

              ((cartesian_waypoints_planner_B.loop_ub < 0) +

               cartesian_waypoints_planner_B.loop_ub) >> 1;

            cartesian_waypoints_planner_B.c_o =

              cartesian_waypoints_planner_B.nm1d2 - 2;

            for (cartesian_waypoints_planner_B.partialTrueCount = 0;

                 cartesian_waypoints_planner_B.partialTrueCount <=

                 cartesian_waypoints_planner_B.c_o;

                 cartesian_waypoints_planner_B.partialTrueCount++) {

              cartesian_waypoints_planner_B.k_j =

                cartesian_waypoints_planner_B.partialTrueCount + 1;

              e->data[cartesian_waypoints_planner_B.k_j] = positionMap->

                data[cartesian_waypoints_planner_B.i_c] + static_cast<real_T>

                (cartesian_waypoints_planner_B.k_j);

              e->data[cartesian_waypoints_planner_B.loop_ub -

                cartesian_waypoints_planner_B.k_j] =

                cartesian_waypoints_planner_B.apnd - static_cast<real_T>

                (cartesian_waypoints_planner_B.k_j);

            }



            if (cartesian_waypoints_planner_B.nm1d2 << 1 ==

                cartesian_waypoints_planner_B.loop_ub) {

              e->data[cartesian_waypoints_planner_B.nm1d2] = (positionMap->

                data[cartesian_waypoints_planner_B.i_c] +

                cartesian_waypoints_planner_B.apnd) / 2.0;

            } else {

              e->data[cartesian_waypoints_planner_B.nm1d2] = positionMap->

                data[cartesian_waypoints_planner_B.i_c] + static_cast<real_T>

                (cartesian_waypoints_planner_B.nm1d2);

              e->data[cartesian_waypoints_planner_B.nm1d2 + 1] =

                cartesian_waypoints_planner_B.apnd - static_cast<real_T>

                (cartesian_waypoints_planner_B.nm1d2);

            }

          }

        }

      }



      cartesian_waypoints_planner_B.partialTrueCount = e->size[0] * e->size[1];

      cartesian_waypoints_planner_B.loop_ub =

        cartesian_waypoints_planner_B.partialTrueCount - 1;

      for (cartesian_waypoints_planner_B.partialTrueCount = 0;

           cartesian_waypoints_planner_B.partialTrueCount <=

           cartesian_waypoints_planner_B.loop_ub;

           cartesian_waypoints_planner_B.partialTrueCount++) {

        positionIndices->data[static_cast<int32_T>

          (cartesian_waypoints_planner_B.bid + y->

           data[cartesian_waypoints_planner_B.partialTrueCount]) - 1] = e->

          data[cartesian_waypoints_planner_B.partialTrueCount];

      }



      cartesian_waypoints_planner_B.bid +=

        cartesian_waypoints_planner_B.numPositions;

    }

  }



  cartesian_waypoi_emxFree_real_T(&y);

  cartesian_waypoi_emxFree_real_T(&e);

  cartesian_waypoi_emxFree_real_T(&positionMap);

  if (1.0 > cartesian_waypoints_planner_B.bid) {

    positionIndices->size[1] = 0;

  } else {

    cartesian_waypoints_planner_B.partialTrueCount = positionIndices->size[0] *

      positionIndices->size[1];

    positionIndices->size[1] = static_cast<int32_T>

      (cartesian_waypoints_planner_B.bid);

    cartes_emxEnsureCapacity_real_T(positionIndices,

      cartesian_waypoints_planner_B.partialTrueCount);

  }



  cartesian_waypoints_planner_B.loop_ub = positionIndices->size[0] *

    positionIndices->size[1];

  for (cartesian_waypoints_planner_B.partialTrueCount = 0;

       cartesian_waypoints_planner_B.partialTrueCount <

       cartesian_waypoints_planner_B.loop_ub;

       cartesian_waypoints_planner_B.partialTrueCount++) {

    QSol[static_cast<int32_T>(positionIndices->

      data[cartesian_waypoints_planner_B.partialTrueCount]) - 1] =

      cartesian_waypoints_planner_B.qvSolRaw[static_cast<int32_T>

      (positionIndices->data[cartesian_waypoints_planner_B.partialTrueCount]) -

      1];

  }



  cartesian_waypoi_emxFree_real_T(&positionIndices);

}



static void cartesian_w_emxInit_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T

  **pEmxArray, int32_T numDimensions)

{

  emxArray_f_cell_wrap_cartesia_T *emxArray;

  int32_T i;

  *pEmxArray = (emxArray_f_cell_wrap_cartesia_T *)malloc(sizeof

    (emxArray_f_cell_wrap_cartesia_T));

  emxArray = *pEmxArray;

  emxArray->data = (f_cell_wrap_cartesian_waypoin_T *)NULL;

  emxArray->numDimensions = numDimensions;

  emxArray->size = (int32_T *)malloc(sizeof(int32_T) * numDimensions);

  emxArray->allocatedSize = 0;

  emxArray->canFreeData = true;

  for (i = 0; i < numDimensions; i++) {

    emxArray->size[i] = 0;

  }

}



static void c_emxEnsureCapacity_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T

  *emxArray, int32_T oldNumel)

{

  int32_T newNumel;

  int32_T i;

  void *newData;

  if (oldNumel < 0) {

    oldNumel = 0;

  }



  newNumel = 1;

  for (i = 0; i < emxArray->numDimensions; i++) {

    newNumel *= emxArray->size[i];

  }



  if (newNumel > emxArray->allocatedSize) {

    i = emxArray->allocatedSize;

    if (i < 16) {

      i = 16;

    }



    while (i < newNumel) {

      if (i > 1073741823) {

        i = MAX_int32_T;

      } else {

        i <<= 1;

      }

    }



    newData = calloc(static_cast<uint32_T>(i), sizeof

                     (f_cell_wrap_cartesian_waypoin_T));

    if (emxArray->data != NULL) {

      memcpy(newData, emxArray->data, sizeof(f_cell_wrap_cartesian_waypoin_T)

             * oldNumel);

      if (emxArray->canFreeData) {

        free(emxArray->data);

      }

    }



    emxArray->data = (f_cell_wrap_cartesian_waypoin_T *)newData;

    emxArray->allocatedSize = i;

    emxArray->canFreeData = true;

  }

}



static void rigidBodyJoint_get_JointAxis_e(const c_rigidBodyJoint_cartesian__e_T

  *obj, real_T ax[3])

{

  emxArray_char_T_cartesian_way_T *a;

  static const char_T tmp[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_0[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  boolean_T guard1 = false;

  int32_T exitg1;

  cartesian_waypoi_emxInit_char_T(&a, 2);

  cartesian_waypoints_planner_B.b_kstr_h = a->size[0] * a->size[1];

  a->size[0] = 1;

  a->size[1] = obj->Type->size[1];

  cartes_emxEnsureCapacity_char_T(a, cartesian_waypoints_planner_B.b_kstr_h);

  cartesian_waypoints_planner_B.loop_ub_d1 = obj->Type->size[0] * obj->

    Type->size[1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr_h = 0;

       cartesian_waypoints_planner_B.b_kstr_h <=

       cartesian_waypoints_planner_B.loop_ub_d1;

       cartesian_waypoints_planner_B.b_kstr_h++) {

    a->data[cartesian_waypoints_planner_B.b_kstr_h] = obj->Type->

      data[cartesian_waypoints_planner_B.b_kstr_h];

  }



  for (cartesian_waypoints_planner_B.b_kstr_h = 0;

       cartesian_waypoints_planner_B.b_kstr_h < 8;

       cartesian_waypoints_planner_B.b_kstr_h++) {

    cartesian_waypoints_planner_B.b_fx[cartesian_waypoints_planner_B.b_kstr_h] =

      tmp[cartesian_waypoints_planner_B.b_kstr_h];

  }



  cartesian_waypoints_planner_B.b_bool_cg = false;

  if (a->size[1] == 8) {

    cartesian_waypoints_planner_B.b_kstr_h = 1;

    do {

      exitg1 = 0;

      if (cartesian_waypoints_planner_B.b_kstr_h - 1 < 8) {

        cartesian_waypoints_planner_B.loop_ub_d1 =

          cartesian_waypoints_planner_B.b_kstr_h - 1;

        if (a->data[cartesian_waypoints_planner_B.loop_ub_d1] !=

            cartesian_waypoints_planner_B.b_fx[cartesian_waypoints_planner_B.loop_ub_d1])

        {

          exitg1 = 1;

        } else {

          cartesian_waypoints_planner_B.b_kstr_h++;

        }

      } else {

        cartesian_waypoints_planner_B.b_bool_cg = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  guard1 = false;

  if (cartesian_waypoints_planner_B.b_bool_cg) {

    guard1 = true;

  } else {

    cartesian_waypoints_planner_B.b_kstr_h = a->size[0] * a->size[1];

    a->size[0] = 1;

    a->size[1] = obj->Type->size[1];

    cartes_emxEnsureCapacity_char_T(a, cartesian_waypoints_planner_B.b_kstr_h);

    cartesian_waypoints_planner_B.loop_ub_d1 = obj->Type->size[0] * obj->

      Type->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_kstr_h = 0;

         cartesian_waypoints_planner_B.b_kstr_h <=

         cartesian_waypoints_planner_B.loop_ub_d1;

         cartesian_waypoints_planner_B.b_kstr_h++) {

      a->data[cartesian_waypoints_planner_B.b_kstr_h] = obj->Type->

        data[cartesian_waypoints_planner_B.b_kstr_h];

    }



    for (cartesian_waypoints_planner_B.b_kstr_h = 0;

         cartesian_waypoints_planner_B.b_kstr_h < 9;

         cartesian_waypoints_planner_B.b_kstr_h++) {

      cartesian_waypoints_planner_B.b_mv[cartesian_waypoints_planner_B.b_kstr_h]

        = tmp_0[cartesian_waypoints_planner_B.b_kstr_h];

    }



    cartesian_waypoints_planner_B.b_bool_cg = false;

    if (a->size[1] == 9) {

      cartesian_waypoints_planner_B.b_kstr_h = 1;

      do {

        exitg1 = 0;

        if (cartesian_waypoints_planner_B.b_kstr_h - 1 < 9) {

          cartesian_waypoints_planner_B.loop_ub_d1 =

            cartesian_waypoints_planner_B.b_kstr_h - 1;

          if (a->data[cartesian_waypoints_planner_B.loop_ub_d1] !=

              cartesian_waypoints_planner_B.b_mv[cartesian_waypoints_planner_B.loop_ub_d1])

          {

            exitg1 = 1;

          } else {

            cartesian_waypoints_planner_B.b_kstr_h++;

          }

        } else {

          cartesian_waypoints_planner_B.b_bool_cg = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (cartesian_waypoints_planner_B.b_bool_cg) {

      guard1 = true;

    } else {

      ax[0] = (rtNaN);

      ax[1] = (rtNaN);

      ax[2] = (rtNaN);

    }

  }



  if (guard1) {

    ax[0] = obj->JointAxisInternal[0];

    ax[1] = obj->JointAxisInternal[1];

    ax[2] = obj->JointAxisInternal[2];

  }



  cartesian_waypoi_emxFree_char_T(&a);

}



static void RigidBodyTree_forwardKinematics(p_robotics_manip_internal_Rig_T *obj,

  const real_T qvec[6], emxArray_f_cell_wrap_cartesia_T *Ttree)

{

  n_robotics_manip_internal_Rig_T *body;

  emxArray_char_T_cartesian_way_T *switch_expression;

  static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1

  };



  static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };



  static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  int32_T exitg1;

  cartesian_waypoints_planner_B.n = obj->NumBodies;

  for (cartesian_waypoints_planner_B.b_kstr_jk = 0;

       cartesian_waypoints_planner_B.b_kstr_jk < 16;

       cartesian_waypoints_planner_B.b_kstr_jk++) {

    cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.b_kstr_jk] =

      tmp[cartesian_waypoints_planner_B.b_kstr_jk];

  }



  cartesian_waypoints_planner_B.b_kstr_jk = Ttree->size[0] * Ttree->size[1];

  Ttree->size[0] = 1;

  cartesian_waypoints_planner_B.e_ci = static_cast<int32_T>

    (cartesian_waypoints_planner_B.n);

  Ttree->size[1] = cartesian_waypoints_planner_B.e_ci;

  c_emxEnsureCapacity_f_cell_wrap(Ttree, cartesian_waypoints_planner_B.b_kstr_jk);

  if (cartesian_waypoints_planner_B.e_ci != 0) {

    cartesian_waypoints_planner_B.ntilecols = cartesian_waypoints_planner_B.e_ci

      - 1;

    if (0 <= cartesian_waypoints_planner_B.ntilecols) {

      memcpy(&cartesian_waypoints_planner_B.expl_temp.f1[0],

             &cartesian_waypoints_planner_B.c_f1[0], sizeof(real_T) << 4U);

    }



    for (cartesian_waypoints_planner_B.b_jtilecol = 0;

         cartesian_waypoints_planner_B.b_jtilecol <=

         cartesian_waypoints_planner_B.ntilecols;

         cartesian_waypoints_planner_B.b_jtilecol++) {

      Ttree->data[cartesian_waypoints_planner_B.b_jtilecol] =

        cartesian_waypoints_planner_B.expl_temp;

    }

  }



  cartesian_waypoints_planner_B.k = 1.0;

  cartesian_waypoints_planner_B.ntilecols = static_cast<int32_T>

    (cartesian_waypoints_planner_B.n) - 1;

  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  if (0 <= cartesian_waypoints_planner_B.ntilecols) {

    for (cartesian_waypoints_planner_B.b_kstr_jk = 0;

         cartesian_waypoints_planner_B.b_kstr_jk < 5;

         cartesian_waypoints_planner_B.b_kstr_jk++) {

      cartesian_waypoints_planner_B.b_kl[cartesian_waypoints_planner_B.b_kstr_jk]

        = tmp_0[cartesian_waypoints_planner_B.b_kstr_jk];

    }

  }



  for (cartesian_waypoints_planner_B.b_jtilecol = 0;

       cartesian_waypoints_planner_B.b_jtilecol <=

       cartesian_waypoints_planner_B.ntilecols;

       cartesian_waypoints_planner_B.b_jtilecol++) {

    body = obj->Bodies[cartesian_waypoints_planner_B.b_jtilecol];

    cartesian_waypoints_planner_B.n = body->JointInternal.PositionNumber;

    cartesian_waypoints_planner_B.n += cartesian_waypoints_planner_B.k;

    if (cartesian_waypoints_planner_B.k > cartesian_waypoints_planner_B.n - 1.0)

    {

      cartesian_waypoints_planner_B.e_ci = 0;

      cartesian_waypoints_planner_B.d_f = 0;

    } else {

      cartesian_waypoints_planner_B.e_ci = static_cast<int32_T>

        (cartesian_waypoints_planner_B.k) - 1;

      cartesian_waypoints_planner_B.d_f = static_cast<int32_T>

        (cartesian_waypoints_planner_B.n - 1.0);

    }



    cartesian_waypoints_planner_B.b_kstr_jk = switch_expression->size[0] *

      switch_expression->size[1];

    switch_expression->size[0] = 1;

    switch_expression->size[1] = body->JointInternal.Type->size[1];

    cartes_emxEnsureCapacity_char_T(switch_expression,

      cartesian_waypoints_planner_B.b_kstr_jk);

    cartesian_waypoints_planner_B.loop_ub_k = body->JointInternal.Type->size[0] *

      body->JointInternal.Type->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_kstr_jk = 0;

         cartesian_waypoints_planner_B.b_kstr_jk <=

         cartesian_waypoints_planner_B.loop_ub_k;

         cartesian_waypoints_planner_B.b_kstr_jk++) {

      switch_expression->data[cartesian_waypoints_planner_B.b_kstr_jk] =

        body->JointInternal.Type->data[cartesian_waypoints_planner_B.b_kstr_jk];

    }



    cartesian_waypoints_planner_B.b_bool_k = false;

    if (switch_expression->size[1] == 5) {

      cartesian_waypoints_planner_B.b_kstr_jk = 1;

      do {

        exitg1 = 0;

        if (cartesian_waypoints_planner_B.b_kstr_jk - 1 < 5) {

          cartesian_waypoints_planner_B.loop_ub_k =

            cartesian_waypoints_planner_B.b_kstr_jk - 1;

          if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_k]

              !=

              cartesian_waypoints_planner_B.b_kl[cartesian_waypoints_planner_B.loop_ub_k])

          {

            exitg1 = 1;

          } else {

            cartesian_waypoints_planner_B.b_kstr_jk++;

          }

        } else {

          cartesian_waypoints_planner_B.b_bool_k = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (cartesian_waypoints_planner_B.b_bool_k) {

      cartesian_waypoints_planner_B.b_kstr_jk = 0;

    } else {

      for (cartesian_waypoints_planner_B.b_kstr_jk = 0;

           cartesian_waypoints_planner_B.b_kstr_jk < 8;

           cartesian_waypoints_planner_B.b_kstr_jk++) {

        cartesian_waypoints_planner_B.b_c[cartesian_waypoints_planner_B.b_kstr_jk]

          = tmp_1[cartesian_waypoints_planner_B.b_kstr_jk];

      }



      cartesian_waypoints_planner_B.b_bool_k = false;

      if (switch_expression->size[1] == 8) {

        cartesian_waypoints_planner_B.b_kstr_jk = 1;

        do {

          exitg1 = 0;

          if (cartesian_waypoints_planner_B.b_kstr_jk - 1 < 8) {

            cartesian_waypoints_planner_B.loop_ub_k =

              cartesian_waypoints_planner_B.b_kstr_jk - 1;

            if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_k]

                !=

                cartesian_waypoints_planner_B.b_c[cartesian_waypoints_planner_B.loop_ub_k])

            {

              exitg1 = 1;

            } else {

              cartesian_waypoints_planner_B.b_kstr_jk++;

            }

          } else {

            cartesian_waypoints_planner_B.b_bool_k = true;

            exitg1 = 1;

          }

        } while (exitg1 == 0);

      }



      if (cartesian_waypoints_planner_B.b_bool_k) {

        cartesian_waypoints_planner_B.b_kstr_jk = 1;

      } else {

        cartesian_waypoints_planner_B.b_kstr_jk = -1;

      }

    }



    switch (cartesian_waypoints_planner_B.b_kstr_jk) {

     case 0:

      memset(&cartesian_waypoints_planner_B.c_f1[0], 0, sizeof(real_T) << 4U);

      cartesian_waypoints_planner_B.c_f1[0] = 1.0;

      cartesian_waypoints_planner_B.c_f1[5] = 1.0;

      cartesian_waypoints_planner_B.c_f1[10] = 1.0;

      cartesian_waypoints_planner_B.c_f1[15] = 1.0;

      break;



     case 1:

      rigidBodyJoint_get_JointAxis_e(&body->JointInternal,

        cartesian_waypoints_planner_B.v_kb);

      cartesian_waypoints_planner_B.d_f -= cartesian_waypoints_planner_B.e_ci;

      for (cartesian_waypoints_planner_B.b_kstr_jk = 0;

           cartesian_waypoints_planner_B.b_kstr_jk <

           cartesian_waypoints_planner_B.d_f;

           cartesian_waypoints_planner_B.b_kstr_jk++) {

        cartesian_waypoints_planner_B.e_data[cartesian_waypoints_planner_B.b_kstr_jk]

          = cartesian_waypoints_planner_B.e_ci +

          cartesian_waypoints_planner_B.b_kstr_jk;

      }



      cartesian_waypoints_planner_B.result_data_o[0] =

        cartesian_waypoints_planner_B.v_kb[0];

      cartesian_waypoints_planner_B.result_data_o[1] =

        cartesian_waypoints_planner_B.v_kb[1];

      cartesian_waypoints_planner_B.result_data_o[2] =

        cartesian_waypoints_planner_B.v_kb[2];

      if (0 <= (cartesian_waypoints_planner_B.d_f != 0) - 1) {

        cartesian_waypoints_planner_B.result_data_o[3] =

          qvec[cartesian_waypoints_planner_B.e_data[0]];

      }



      cartesian_waypoints_planner_B.k = 1.0 / sqrt

        ((cartesian_waypoints_planner_B.result_data_o[0] *

          cartesian_waypoints_planner_B.result_data_o[0] +

          cartesian_waypoints_planner_B.result_data_o[1] *

          cartesian_waypoints_planner_B.result_data_o[1]) +

         cartesian_waypoints_planner_B.result_data_o[2] *

         cartesian_waypoints_planner_B.result_data_o[2]);

      cartesian_waypoints_planner_B.v_kb[0] =

        cartesian_waypoints_planner_B.result_data_o[0] *

        cartesian_waypoints_planner_B.k;

      cartesian_waypoints_planner_B.v_kb[1] =

        cartesian_waypoints_planner_B.result_data_o[1] *

        cartesian_waypoints_planner_B.k;

      cartesian_waypoints_planner_B.v_kb[2] =

        cartesian_waypoints_planner_B.result_data_o[2] *

        cartesian_waypoints_planner_B.k;

      cartesian_waypoints_planner_B.k = cos

        (cartesian_waypoints_planner_B.result_data_o[3]);

      cartesian_waypoints_planner_B.sth_i = sin

        (cartesian_waypoints_planner_B.result_data_o[3]);

      cartesian_waypoints_planner_B.tempR_h[0] =

        cartesian_waypoints_planner_B.v_kb[0] *

        cartesian_waypoints_planner_B.v_kb[0] * (1.0 -

        cartesian_waypoints_planner_B.k) + cartesian_waypoints_planner_B.k;

      cartesian_waypoints_planner_B.tempR_tmp_k =

        cartesian_waypoints_planner_B.v_kb[1] *

        cartesian_waypoints_planner_B.v_kb[0] * (1.0 -

        cartesian_waypoints_planner_B.k);

      cartesian_waypoints_planner_B.tempR_tmp_fc =

        cartesian_waypoints_planner_B.v_kb[2] *

        cartesian_waypoints_planner_B.sth_i;

      cartesian_waypoints_planner_B.tempR_h[1] =

        cartesian_waypoints_planner_B.tempR_tmp_k -

        cartesian_waypoints_planner_B.tempR_tmp_fc;

      cartesian_waypoints_planner_B.tempR_tmp_a =

        cartesian_waypoints_planner_B.v_kb[2] *

        cartesian_waypoints_planner_B.v_kb[0] * (1.0 -

        cartesian_waypoints_planner_B.k);

      cartesian_waypoints_planner_B.tempR_tmp_d4 =

        cartesian_waypoints_planner_B.v_kb[1] *

        cartesian_waypoints_planner_B.sth_i;

      cartesian_waypoints_planner_B.tempR_h[2] =

        cartesian_waypoints_planner_B.tempR_tmp_a +

        cartesian_waypoints_planner_B.tempR_tmp_d4;

      cartesian_waypoints_planner_B.tempR_h[3] =

        cartesian_waypoints_planner_B.tempR_tmp_k +

        cartesian_waypoints_planner_B.tempR_tmp_fc;

      cartesian_waypoints_planner_B.tempR_h[4] =

        cartesian_waypoints_planner_B.v_kb[1] *

        cartesian_waypoints_planner_B.v_kb[1] * (1.0 -

        cartesian_waypoints_planner_B.k) + cartesian_waypoints_planner_B.k;

      cartesian_waypoints_planner_B.tempR_tmp_k =

        cartesian_waypoints_planner_B.v_kb[2] *

        cartesian_waypoints_planner_B.v_kb[1] * (1.0 -

        cartesian_waypoints_planner_B.k);

      cartesian_waypoints_planner_B.tempR_tmp_fc =

        cartesian_waypoints_planner_B.v_kb[0] *

        cartesian_waypoints_planner_B.sth_i;

      cartesian_waypoints_planner_B.tempR_h[5] =

        cartesian_waypoints_planner_B.tempR_tmp_k -

        cartesian_waypoints_planner_B.tempR_tmp_fc;

      cartesian_waypoints_planner_B.tempR_h[6] =

        cartesian_waypoints_planner_B.tempR_tmp_a -

        cartesian_waypoints_planner_B.tempR_tmp_d4;

      cartesian_waypoints_planner_B.tempR_h[7] =

        cartesian_waypoints_planner_B.tempR_tmp_k +

        cartesian_waypoints_planner_B.tempR_tmp_fc;

      cartesian_waypoints_planner_B.tempR_h[8] =

        cartesian_waypoints_planner_B.v_kb[2] *

        cartesian_waypoints_planner_B.v_kb[2] * (1.0 -

        cartesian_waypoints_planner_B.k) + cartesian_waypoints_planner_B.k;

      for (cartesian_waypoints_planner_B.b_kstr_jk = 0;

           cartesian_waypoints_planner_B.b_kstr_jk < 3;

           cartesian_waypoints_planner_B.b_kstr_jk++) {

        cartesian_waypoints_planner_B.e_ci =

          cartesian_waypoints_planner_B.b_kstr_jk + 1;

        cartesian_waypoints_planner_B.R_l5[cartesian_waypoints_planner_B.e_ci -

          1] = cartesian_waypoints_planner_B.tempR_h

          [(cartesian_waypoints_planner_B.e_ci - 1) * 3];

        cartesian_waypoints_planner_B.e_ci =

          cartesian_waypoints_planner_B.b_kstr_jk + 1;

        cartesian_waypoints_planner_B.R_l5[cartesian_waypoints_planner_B.e_ci +

          2] = cartesian_waypoints_planner_B.tempR_h

          [(cartesian_waypoints_planner_B.e_ci - 1) * 3 + 1];

        cartesian_waypoints_planner_B.e_ci =

          cartesian_waypoints_planner_B.b_kstr_jk + 1;

        cartesian_waypoints_planner_B.R_l5[cartesian_waypoints_planner_B.e_ci +

          5] = cartesian_waypoints_planner_B.tempR_h

          [(cartesian_waypoints_planner_B.e_ci - 1) * 3 + 2];

      }



      memset(&cartesian_waypoints_planner_B.c_f1[0], 0, sizeof(real_T) << 4U);

      for (cartesian_waypoints_planner_B.b_kstr_jk = 0;

           cartesian_waypoints_planner_B.b_kstr_jk < 3;

           cartesian_waypoints_planner_B.b_kstr_jk++) {

        cartesian_waypoints_planner_B.d_f =

          cartesian_waypoints_planner_B.b_kstr_jk << 2;

        cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_f] =

          cartesian_waypoints_planner_B.R_l5[3 *

          cartesian_waypoints_planner_B.b_kstr_jk];

        cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_f + 1]

          = cartesian_waypoints_planner_B.R_l5[3 *

          cartesian_waypoints_planner_B.b_kstr_jk + 1];

        cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_f + 2]

          = cartesian_waypoints_planner_B.R_l5[3 *

          cartesian_waypoints_planner_B.b_kstr_jk + 2];

      }



      cartesian_waypoints_planner_B.c_f1[15] = 1.0;

      break;



     default:

      rigidBodyJoint_get_JointAxis_e(&body->JointInternal,

        cartesian_waypoints_planner_B.v_kb);

      memset(&cartesian_waypoints_planner_B.tempR_h[0], 0, 9U * sizeof(real_T));

      cartesian_waypoints_planner_B.tempR_h[0] = 1.0;

      cartesian_waypoints_planner_B.tempR_h[4] = 1.0;

      cartesian_waypoints_planner_B.tempR_h[8] = 1.0;

      for (cartesian_waypoints_planner_B.b_kstr_jk = 0;

           cartesian_waypoints_planner_B.b_kstr_jk < 3;

           cartesian_waypoints_planner_B.b_kstr_jk++) {

        cartesian_waypoints_planner_B.d_f =

          cartesian_waypoints_planner_B.b_kstr_jk << 2;

        cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_f] =

          cartesian_waypoints_planner_B.tempR_h[3 *

          cartesian_waypoints_planner_B.b_kstr_jk];

        cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_f + 1]

          = cartesian_waypoints_planner_B.tempR_h[3 *

          cartesian_waypoints_planner_B.b_kstr_jk + 1];

        cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_f + 2]

          = cartesian_waypoints_planner_B.tempR_h[3 *

          cartesian_waypoints_planner_B.b_kstr_jk + 2];

        cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.b_kstr_jk

          + 12] =

          cartesian_waypoints_planner_B.v_kb[cartesian_waypoints_planner_B.b_kstr_jk]

          * qvec[cartesian_waypoints_planner_B.e_ci];

      }



      cartesian_waypoints_planner_B.c_f1[3] = 0.0;

      cartesian_waypoints_planner_B.c_f1[7] = 0.0;

      cartesian_waypoints_planner_B.c_f1[11] = 0.0;

      cartesian_waypoints_planner_B.c_f1[15] = 1.0;

      break;

    }



    for (cartesian_waypoints_planner_B.b_kstr_jk = 0;

         cartesian_waypoints_planner_B.b_kstr_jk < 16;

         cartesian_waypoints_planner_B.b_kstr_jk++) {

      cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_jk]

        = body->

        JointInternal.JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_jk];

    }



    for (cartesian_waypoints_planner_B.b_kstr_jk = 0;

         cartesian_waypoints_planner_B.b_kstr_jk < 16;

         cartesian_waypoints_planner_B.b_kstr_jk++) {

      cartesian_waypoints_planner_B.b_n[cartesian_waypoints_planner_B.b_kstr_jk]

        = body->

        JointInternal.ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_jk];

    }



    for (cartesian_waypoints_planner_B.b_kstr_jk = 0;

         cartesian_waypoints_planner_B.b_kstr_jk < 4;

         cartesian_waypoints_planner_B.b_kstr_jk++) {

      for (cartesian_waypoints_planner_B.e_ci = 0;

           cartesian_waypoints_planner_B.e_ci < 4;

           cartesian_waypoints_planner_B.e_ci++) {

        cartesian_waypoints_planner_B.d_f = cartesian_waypoints_planner_B.e_ci <<

          2;

        cartesian_waypoints_planner_B.loop_ub_k =

          cartesian_waypoints_planner_B.b_kstr_jk +

          cartesian_waypoints_planner_B.d_f;

        cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.loop_ub_k]

          = 0.0;

        cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.loop_ub_k]

          +=

          cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_f] *

          cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_jk];

        cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.loop_ub_k]

          +=

          cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_f +

          1] *

          cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_jk

          + 4];

        cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.loop_ub_k]

          +=

          cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_f +

          2] *

          cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_jk

          + 8];

        cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.loop_ub_k]

          +=

          cartesian_waypoints_planner_B.c_f1[cartesian_waypoints_planner_B.d_f +

          3] *

          cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_jk

          + 12];

      }



      for (cartesian_waypoints_planner_B.e_ci = 0;

           cartesian_waypoints_planner_B.e_ci < 4;

           cartesian_waypoints_planner_B.e_ci++) {

        cartesian_waypoints_planner_B.d_f = cartesian_waypoints_planner_B.e_ci <<

          2;

        cartesian_waypoints_planner_B.loop_ub_k =

          cartesian_waypoints_planner_B.b_kstr_jk +

          cartesian_waypoints_planner_B.d_f;

        Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]

          .f1[cartesian_waypoints_planner_B.loop_ub_k] = 0.0;

        Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]

          .f1[cartesian_waypoints_planner_B.loop_ub_k] +=

          cartesian_waypoints_planner_B.b_n[cartesian_waypoints_planner_B.d_f] *

          cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.b_kstr_jk];

        Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]

          .f1[cartesian_waypoints_planner_B.loop_ub_k] +=

          cartesian_waypoints_planner_B.b_n[cartesian_waypoints_planner_B.d_f +

          1] *

          cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.b_kstr_jk

          + 4];

        Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]

          .f1[cartesian_waypoints_planner_B.loop_ub_k] +=

          cartesian_waypoints_planner_B.b_n[cartesian_waypoints_planner_B.d_f +

          2] *

          cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.b_kstr_jk

          + 8];

        Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]

          .f1[cartesian_waypoints_planner_B.loop_ub_k] +=

          cartesian_waypoints_planner_B.b_n[cartesian_waypoints_planner_B.d_f +

          3] *

          cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.b_kstr_jk

          + 12];

      }

    }



    cartesian_waypoints_planner_B.k = cartesian_waypoints_planner_B.n;

    if (body->ParentIndex > 0.0) {

      for (cartesian_waypoints_planner_B.b_kstr_jk = 0;

           cartesian_waypoints_planner_B.b_kstr_jk < 16;

           cartesian_waypoints_planner_B.b_kstr_jk++) {

        cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_jk]

          = Ttree->data[static_cast<int32_T>(body->ParentIndex) - 1]

          .f1[cartesian_waypoints_planner_B.b_kstr_jk];

      }



      for (cartesian_waypoints_planner_B.b_kstr_jk = 0;

           cartesian_waypoints_planner_B.b_kstr_jk < 4;

           cartesian_waypoints_planner_B.b_kstr_jk++) {

        for (cartesian_waypoints_planner_B.e_ci = 0;

             cartesian_waypoints_planner_B.e_ci < 4;

             cartesian_waypoints_planner_B.e_ci++) {

          cartesian_waypoints_planner_B.d_f = cartesian_waypoints_planner_B.e_ci

            << 2;

          cartesian_waypoints_planner_B.loop_ub_k =

            cartesian_waypoints_planner_B.b_kstr_jk +

            cartesian_waypoints_planner_B.d_f;

          cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.loop_ub_k]

            = 0.0;

          cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.loop_ub_k]

            += Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]

            .f1[cartesian_waypoints_planner_B.d_f] *

            cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_jk];

          cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.loop_ub_k]

            += Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]

            .f1[cartesian_waypoints_planner_B.d_f + 1] *

            cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_jk

            + 4];

          cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.loop_ub_k]

            += Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]

            .f1[cartesian_waypoints_planner_B.d_f + 2] *

            cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_jk

            + 8];

          cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.loop_ub_k]

            += Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]

            .f1[cartesian_waypoints_planner_B.d_f + 3] *

            cartesian_waypoints_planner_B.a_o[cartesian_waypoints_planner_B.b_kstr_jk

            + 12];

        }

      }



      for (cartesian_waypoints_planner_B.b_kstr_jk = 0;

           cartesian_waypoints_planner_B.b_kstr_jk < 16;

           cartesian_waypoints_planner_B.b_kstr_jk++) {

        Ttree->data[cartesian_waypoints_planner_B.b_jtilecol]

          .f1[cartesian_waypoints_planner_B.b_kstr_jk] =

          cartesian_waypoints_planner_B.a_m[cartesian_waypoints_planner_B.b_kstr_jk];

      }

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

}



static void cartesian_w_emxFree_f_cell_wrap(emxArray_f_cell_wrap_cartesia_T

  **pEmxArray)

{

  if (*pEmxArray != (emxArray_f_cell_wrap_cartesia_T *)NULL) {

    if (((*pEmxArray)->data != (f_cell_wrap_cartesian_waypoin_T *)NULL) &&

        (*pEmxArray)->canFreeData) {

      free((*pEmxArray)->data);

    }



    free((*pEmxArray)->size);

    free(*pEmxArray);

    *pEmxArray = (emxArray_f_cell_wrap_cartesia_T *)NULL;

  }

}



// Function for MATLAB Function: '<S9>/MATLAB Function5'

static boolean_T cartesian_waypoint_anyNonFinite(const real_T x[16])

{

  boolean_T b_p;

  int32_T k;

  b_p = true;

  for (k = 0; k < 16; k++) {

    if (b_p && (rtIsInf(x[k]) || rtIsNaN(x[k]))) {

      b_p = false;

    }

  }



  return !b_p;

}



// Function for MATLAB Function: '<S9>/MATLAB Function5'

static real_T cartesian_waypoints_pl_xzlangeM(const creal_T x[16])

{

  real_T y;

  real_T absxk;

  int32_T k;

  boolean_T exitg1;

  y = 0.0;

  k = 0;

  exitg1 = false;

  while ((!exitg1) && (k < 16)) {

    absxk = cartesian_waypoin_rt_hypotd_snf(x[k].re, x[k].im);

    if (rtIsNaN(absxk)) {

      y = (rtNaN);

      exitg1 = true;

    } else {

      if (absxk > y) {

        y = absxk;

      }



      k++;

    }

  }



  return y;

}



// Function for MATLAB Function: '<S9>/MATLAB Function5'

static void cartesian_waypoints_pla_xzlascl(real_T cfrom, real_T cto, creal_T A

  [16])

{

  real_T cfromc;

  real_T ctoc;

  boolean_T notdone;

  real_T cfrom1;

  real_T cto1;

  real_T mul;

  int32_T i;

  cfromc = cfrom;

  ctoc = cto;

  notdone = true;

  while (notdone) {

    cfrom1 = cfromc * 2.0041683600089728E-292;

    cto1 = ctoc / 4.9896007738368E+291;

    if ((fabs(cfrom1) > fabs(ctoc)) && (ctoc != 0.0)) {

      mul = 2.0041683600089728E-292;

      cfromc = cfrom1;

    } else if (fabs(cto1) > fabs(cfromc)) {

      mul = 4.9896007738368E+291;

      ctoc = cto1;

    } else {

      mul = ctoc / cfromc;

      notdone = false;

    }



    for (i = 0; i < 16; i++) {

      A[i].re *= mul;

      A[i].im *= mul;

    }

  }

}



// Function for MATLAB Function: '<S9>/MATLAB Function5'

static real_T cartesian_waypoints_pla_xzlanhs(const creal_T A[16], int32_T ilo,

  int32_T ihi)

{

  real_T f;

  real_T scale;

  real_T sumsq;

  boolean_T firstNonZero;

  real_T temp1;

  real_T temp2;

  int32_T j;

  int32_T b;

  int32_T i;

  int32_T reAij_tmp;

  f = 0.0;

  if (ilo <= ihi) {

    scale = 0.0;

    sumsq = 0.0;

    firstNonZero = true;

    for (j = ilo; j <= ihi; j++) {

      b = j + 1;

      if (ihi < j + 1) {

        b = ihi;

      }



      for (i = ilo; i <= b; i++) {

        reAij_tmp = (((j - 1) << 2) + i) - 1;

        if (A[reAij_tmp].re != 0.0) {

          temp1 = fabs(A[reAij_tmp].re);

          if (firstNonZero) {

            sumsq = 1.0;

            scale = temp1;

            firstNonZero = false;

          } else if (scale < temp1) {

            temp2 = scale / temp1;

            sumsq = sumsq * temp2 * temp2 + 1.0;

            scale = temp1;

          } else {

            temp2 = temp1 / scale;

            sumsq += temp2 * temp2;

          }

        }



        if (A[reAij_tmp].im != 0.0) {

          temp1 = fabs(A[reAij_tmp].im);

          if (firstNonZero) {

            sumsq = 1.0;

            scale = temp1;

            firstNonZero = false;

          } else if (scale < temp1) {

            temp2 = scale / temp1;

            sumsq = sumsq * temp2 * temp2 + 1.0;

            scale = temp1;

          } else {

            temp2 = temp1 / scale;

            sumsq += temp2 * temp2;

          }

        }

      }

    }



    f = scale * sqrt(sumsq);

  }



  return f;

}



// Function for MATLAB Function: '<S9>/MATLAB Function5'

static void cartesian_waypoints_p_xzlartg_i(const creal_T f, const creal_T g,

  real_T *cs, creal_T *sn)

{

  real_T scale;

  real_T g2;

  real_T f2s;

  real_T di;

  real_T x;

  real_T fs_re;

  real_T fs_im;

  real_T gs_re;

  real_T gs_im;

  boolean_T guard1 = false;

  f2s = fabs(f.re);

  di = fabs(f.im);

  scale = f2s;

  if (di > f2s) {

    scale = di;

  }



  gs_re = fabs(g.re);

  gs_im = fabs(g.im);

  if (gs_im > gs_re) {

    gs_re = gs_im;

  }



  if (gs_re > scale) {

    scale = gs_re;

  }



  fs_re = f.re;

  fs_im = f.im;

  gs_re = g.re;

  gs_im = g.im;

  guard1 = false;

  if (scale >= 7.4428285367870146E+137) {

    do {

      fs_re *= 1.3435752215134178E-138;

      fs_im *= 1.3435752215134178E-138;

      gs_re *= 1.3435752215134178E-138;

      gs_im *= 1.3435752215134178E-138;

      scale *= 1.3435752215134178E-138;

    } while (!(scale < 7.4428285367870146E+137));



    guard1 = true;

  } else if (scale <= 1.3435752215134178E-138) {

    if ((g.re == 0.0) && (g.im == 0.0)) {

      *cs = 1.0;

      sn->re = 0.0;

      sn->im = 0.0;

    } else {

      do {

        fs_re *= 7.4428285367870146E+137;

        fs_im *= 7.4428285367870146E+137;

        gs_re *= 7.4428285367870146E+137;

        gs_im *= 7.4428285367870146E+137;

        scale *= 7.4428285367870146E+137;

      } while (!(scale > 1.3435752215134178E-138));



      guard1 = true;

    }

  } else {

    guard1 = true;

  }



  if (guard1) {

    scale = fs_re * fs_re + fs_im * fs_im;

    g2 = gs_re * gs_re + gs_im * gs_im;

    x = g2;

    if (1.0 > g2) {

      x = 1.0;

    }



    if (scale <= x * 2.0041683600089728E-292) {

      if ((f.re == 0.0) && (f.im == 0.0)) {

        *cs = 0.0;

        f2s = cartesian_waypoin_rt_hypotd_snf(gs_re, gs_im);

        sn->re = gs_re / f2s;

        sn->im = -gs_im / f2s;

      } else {

        scale = sqrt(g2);

        *cs = cartesian_waypoin_rt_hypotd_snf(fs_re, fs_im) / scale;

        if (di > f2s) {

          f2s = di;

        }



        if (f2s > 1.0) {

          f2s = cartesian_waypoin_rt_hypotd_snf(f.re, f.im);

          fs_re = f.re / f2s;

          fs_im = f.im / f2s;

        } else {

          fs_re = 7.4428285367870146E+137 * f.re;

          di = 7.4428285367870146E+137 * f.im;

          f2s = cartesian_waypoin_rt_hypotd_snf(fs_re, di);

          fs_re /= f2s;

          fs_im = di / f2s;

        }



        gs_re /= scale;

        gs_im = -gs_im / scale;

        sn->re = fs_re * gs_re - fs_im * gs_im;

        sn->im = fs_re * gs_im + fs_im * gs_re;

      }

    } else {

      f2s = sqrt(g2 / scale + 1.0);

      fs_re *= f2s;

      fs_im *= f2s;

      *cs = 1.0 / f2s;

      f2s = scale + g2;

      fs_re /= f2s;

      fs_im /= f2s;

      sn->re = fs_re * gs_re - fs_im * -gs_im;

      sn->im = fs_re * -gs_im + fs_im * gs_re;

    }

  }

}



// Function for MATLAB Function: '<S9>/MATLAB Function5'

static void cartesian_waypoints_pla_xzlartg(const creal_T f, const creal_T g,

  real_T *cs, creal_T *sn, creal_T *r)

{

  int32_T count;

  int32_T rescaledir;

  boolean_T guard1 = false;

  cartesian_waypoints_planner_B.f2s = fabs(f.re);

  cartesian_waypoints_planner_B.di = fabs(f.im);

  cartesian_waypoints_planner_B.scale_n = cartesian_waypoints_planner_B.f2s;

  if (cartesian_waypoints_planner_B.di > cartesian_waypoints_planner_B.f2s) {

    cartesian_waypoints_planner_B.scale_n = cartesian_waypoints_planner_B.di;

  }



  cartesian_waypoints_planner_B.gs_re = fabs(g.re);

  cartesian_waypoints_planner_B.gs_im = fabs(g.im);

  if (cartesian_waypoints_planner_B.gs_im > cartesian_waypoints_planner_B.gs_re)

  {

    cartesian_waypoints_planner_B.gs_re = cartesian_waypoints_planner_B.gs_im;

  }



  if (cartesian_waypoints_planner_B.gs_re >

      cartesian_waypoints_planner_B.scale_n) {

    cartesian_waypoints_planner_B.scale_n = cartesian_waypoints_planner_B.gs_re;

  }



  cartesian_waypoints_planner_B.fs_re = f.re;

  cartesian_waypoints_planner_B.fs_im = f.im;

  cartesian_waypoints_planner_B.gs_re = g.re;

  cartesian_waypoints_planner_B.gs_im = g.im;

  count = -1;

  rescaledir = 0;

  guard1 = false;

  if (cartesian_waypoints_planner_B.scale_n >= 7.4428285367870146E+137) {

    do {

      count++;

      cartesian_waypoints_planner_B.fs_re *= 1.3435752215134178E-138;

      cartesian_waypoints_planner_B.fs_im *= 1.3435752215134178E-138;

      cartesian_waypoints_planner_B.gs_re *= 1.3435752215134178E-138;

      cartesian_waypoints_planner_B.gs_im *= 1.3435752215134178E-138;

      cartesian_waypoints_planner_B.scale_n *= 1.3435752215134178E-138;

    } while (!(cartesian_waypoints_planner_B.scale_n < 7.4428285367870146E+137));



    rescaledir = 1;

    guard1 = true;

  } else if (cartesian_waypoints_planner_B.scale_n <= 1.3435752215134178E-138) {

    if ((g.re == 0.0) && (g.im == 0.0)) {

      *cs = 1.0;

      sn->re = 0.0;

      sn->im = 0.0;

      *r = f;

    } else {

      do {

        count++;

        cartesian_waypoints_planner_B.fs_re *= 7.4428285367870146E+137;

        cartesian_waypoints_planner_B.fs_im *= 7.4428285367870146E+137;

        cartesian_waypoints_planner_B.gs_re *= 7.4428285367870146E+137;

        cartesian_waypoints_planner_B.gs_im *= 7.4428285367870146E+137;

        cartesian_waypoints_planner_B.scale_n *= 7.4428285367870146E+137;

      } while (!(cartesian_waypoints_planner_B.scale_n > 1.3435752215134178E-138));



      rescaledir = -1;

      guard1 = true;

    }

  } else {

    guard1 = true;

  }



  if (guard1) {

    cartesian_waypoints_planner_B.scale_n = cartesian_waypoints_planner_B.fs_re *

      cartesian_waypoints_planner_B.fs_re + cartesian_waypoints_planner_B.fs_im *

      cartesian_waypoints_planner_B.fs_im;

    cartesian_waypoints_planner_B.g2 = cartesian_waypoints_planner_B.gs_re *

      cartesian_waypoints_planner_B.gs_re + cartesian_waypoints_planner_B.gs_im *

      cartesian_waypoints_planner_B.gs_im;

    cartesian_waypoints_planner_B.x_i = cartesian_waypoints_planner_B.g2;

    if (1.0 > cartesian_waypoints_planner_B.g2) {

      cartesian_waypoints_planner_B.x_i = 1.0;

    }



    if (cartesian_waypoints_planner_B.scale_n <=

        cartesian_waypoints_planner_B.x_i * 2.0041683600089728E-292) {

      if ((f.re == 0.0) && (f.im == 0.0)) {

        *cs = 0.0;

        r->re = cartesian_waypoin_rt_hypotd_snf(g.re, g.im);

        r->im = 0.0;

        cartesian_waypoints_planner_B.f2s = cartesian_waypoin_rt_hypotd_snf

          (cartesian_waypoints_planner_B.gs_re,

           cartesian_waypoints_planner_B.gs_im);

        sn->re = cartesian_waypoints_planner_B.gs_re /

          cartesian_waypoints_planner_B.f2s;

        sn->im = -cartesian_waypoints_planner_B.gs_im /

          cartesian_waypoints_planner_B.f2s;

      } else {

        cartesian_waypoints_planner_B.scale_n = sqrt

          (cartesian_waypoints_planner_B.g2);

        *cs = cartesian_waypoin_rt_hypotd_snf

          (cartesian_waypoints_planner_B.fs_re,

           cartesian_waypoints_planner_B.fs_im) /

          cartesian_waypoints_planner_B.scale_n;

        if (cartesian_waypoints_planner_B.di > cartesian_waypoints_planner_B.f2s)

        {

          cartesian_waypoints_planner_B.f2s = cartesian_waypoints_planner_B.di;

        }



        if (cartesian_waypoints_planner_B.f2s > 1.0) {

          cartesian_waypoints_planner_B.f2s = cartesian_waypoin_rt_hypotd_snf

            (f.re, f.im);

          cartesian_waypoints_planner_B.fs_re = f.re /

            cartesian_waypoints_planner_B.f2s;

          cartesian_waypoints_planner_B.fs_im = f.im /

            cartesian_waypoints_planner_B.f2s;

        } else {

          cartesian_waypoints_planner_B.fs_re = 7.4428285367870146E+137 * f.re;

          cartesian_waypoints_planner_B.di = 7.4428285367870146E+137 * f.im;

          cartesian_waypoints_planner_B.f2s = cartesian_waypoin_rt_hypotd_snf

            (cartesian_waypoints_planner_B.fs_re,

             cartesian_waypoints_planner_B.di);

          cartesian_waypoints_planner_B.fs_re /=

            cartesian_waypoints_planner_B.f2s;

          cartesian_waypoints_planner_B.fs_im = cartesian_waypoints_planner_B.di

            / cartesian_waypoints_planner_B.f2s;

        }



        cartesian_waypoints_planner_B.gs_re /=

          cartesian_waypoints_planner_B.scale_n;

        cartesian_waypoints_planner_B.gs_im =

          -cartesian_waypoints_planner_B.gs_im /

          cartesian_waypoints_planner_B.scale_n;

        sn->re = cartesian_waypoints_planner_B.fs_re *

          cartesian_waypoints_planner_B.gs_re -

          cartesian_waypoints_planner_B.fs_im *

          cartesian_waypoints_planner_B.gs_im;

        sn->im = cartesian_waypoints_planner_B.fs_re *

          cartesian_waypoints_planner_B.gs_im +

          cartesian_waypoints_planner_B.fs_im *

          cartesian_waypoints_planner_B.gs_re;

        r->re = (sn->re * g.re - sn->im * g.im) + *cs * f.re;

        r->im = (sn->re * g.im + sn->im * g.re) + *cs * f.im;

      }

    } else {

      cartesian_waypoints_planner_B.f2s = sqrt(cartesian_waypoints_planner_B.g2 /

        cartesian_waypoints_planner_B.scale_n + 1.0);

      r->re = cartesian_waypoints_planner_B.f2s *

        cartesian_waypoints_planner_B.fs_re;

      r->im = cartesian_waypoints_planner_B.f2s *

        cartesian_waypoints_planner_B.fs_im;

      *cs = 1.0 / cartesian_waypoints_planner_B.f2s;

      cartesian_waypoints_planner_B.f2s = cartesian_waypoints_planner_B.scale_n

        + cartesian_waypoints_planner_B.g2;

      cartesian_waypoints_planner_B.fs_re = r->re /

        cartesian_waypoints_planner_B.f2s;

      cartesian_waypoints_planner_B.f2s = r->im /

        cartesian_waypoints_planner_B.f2s;

      sn->re = cartesian_waypoints_planner_B.fs_re *

        cartesian_waypoints_planner_B.gs_re - cartesian_waypoints_planner_B.f2s *

        -cartesian_waypoints_planner_B.gs_im;

      sn->im = cartesian_waypoints_planner_B.fs_re *

        -cartesian_waypoints_planner_B.gs_im + cartesian_waypoints_planner_B.f2s

        * cartesian_waypoints_planner_B.gs_re;

      if (rescaledir > 0) {

        for (rescaledir = 0; rescaledir <= count; rescaledir++) {

          r->re *= 7.4428285367870146E+137;

          r->im *= 7.4428285367870146E+137;

        }

      } else {

        if (rescaledir < 0) {

          for (rescaledir = 0; rescaledir <= count; rescaledir++) {

            r->re *= 1.3435752215134178E-138;

            r->im *= 1.3435752215134178E-138;

          }

        }

      }

    }

  }

}



// Function for MATLAB Function: '<S9>/MATLAB Function5'

static void cartesian_waypoints_pla_xzhgeqz(creal_T A[16], int32_T ilo, int32_T

  ihi, creal_T Z[16], int32_T *info, creal_T alpha1[4], creal_T beta1[4])

{

  boolean_T failed;

  boolean_T goto60;

  boolean_T goto70;

  boolean_T goto90;

  boolean_T guard1 = false;

  boolean_T guard2 = false;

  boolean_T guard3 = false;

  int32_T exitg1;

  boolean_T exitg2;

  *info = 0;

  alpha1[0].re = 0.0;

  alpha1[0].im = 0.0;

  beta1[0].re = 1.0;

  beta1[0].im = 0.0;

  alpha1[1].re = 0.0;

  alpha1[1].im = 0.0;

  beta1[1].re = 1.0;

  beta1[1].im = 0.0;

  alpha1[2].re = 0.0;

  alpha1[2].im = 0.0;

  beta1[2].re = 1.0;

  beta1[2].im = 0.0;

  alpha1[3].re = 0.0;

  alpha1[3].im = 0.0;

  beta1[3].re = 1.0;

  beta1[3].im = 0.0;

  cartesian_waypoints_planner_B.eshift_re = 0.0;

  cartesian_waypoints_planner_B.eshift_im = 0.0;

  cartesian_waypoints_planner_B.ctemp.re = 0.0;

  cartesian_waypoints_planner_B.ctemp.im = 0.0;

  cartesian_waypoints_planner_B.anorm = cartesian_waypoints_pla_xzlanhs(A, ilo,

    ihi);

  cartesian_waypoints_planner_B.shift_re = 2.2204460492503131E-16 *

    cartesian_waypoints_planner_B.anorm;

  cartesian_waypoints_planner_B.b_atol = 2.2250738585072014E-308;

  if (cartesian_waypoints_planner_B.shift_re > 2.2250738585072014E-308) {

    cartesian_waypoints_planner_B.b_atol =

      cartesian_waypoints_planner_B.shift_re;

  }



  cartesian_waypoints_planner_B.shift_re = 2.2250738585072014E-308;

  if (cartesian_waypoints_planner_B.anorm > 2.2250738585072014E-308) {

    cartesian_waypoints_planner_B.shift_re = cartesian_waypoints_planner_B.anorm;

  }



  cartesian_waypoints_planner_B.anorm = 1.0 /

    cartesian_waypoints_planner_B.shift_re;

  failed = true;

  cartesian_waypoints_planner_B.ilast = ihi;

  while (cartesian_waypoints_planner_B.ilast + 1 < 5) {

    alpha1[cartesian_waypoints_planner_B.ilast] = A

      [(cartesian_waypoints_planner_B.ilast << 2) +

      cartesian_waypoints_planner_B.ilast];

    cartesian_waypoints_planner_B.ilast++;

  }



  guard1 = false;

  guard2 = false;

  if (ihi >= ilo) {

    cartesian_waypoints_planner_B.ifirst = ilo;

    cartesian_waypoints_planner_B.istart = ilo;

    cartesian_waypoints_planner_B.ilast = ihi - 1;

    cartesian_waypoints_planner_B.ilastm1 = ihi - 2;

    cartesian_waypoints_planner_B.iiter = 0;

    goto60 = false;

    goto70 = false;

    goto90 = false;

    cartesian_waypoints_planner_B.jiter = 0;

    do {

      exitg1 = 0;

      if (cartesian_waypoints_planner_B.jiter <= ((ihi - ilo) + 1) * 30 - 1) {

        if (cartesian_waypoints_planner_B.ilast + 1 == ilo) {

          goto60 = true;

        } else {

          cartesian_waypoints_planner_B.jp1 =

            (cartesian_waypoints_planner_B.ilastm1 << 2) +

            cartesian_waypoints_planner_B.ilast;

          if (fabs(A[cartesian_waypoints_planner_B.jp1].re) + fabs

              (A[cartesian_waypoints_planner_B.jp1].im) <=

              cartesian_waypoints_planner_B.b_atol) {

            A[cartesian_waypoints_planner_B.jp1].re = 0.0;

            A[cartesian_waypoints_planner_B.jp1].im = 0.0;

            goto60 = true;

          } else {

            cartesian_waypoints_planner_B.j_kt =

              cartesian_waypoints_planner_B.ilastm1;

            guard3 = false;

            exitg2 = false;

            while ((!exitg2) && (cartesian_waypoints_planner_B.j_kt + 1 >= ilo))

            {

              if (cartesian_waypoints_planner_B.j_kt + 1 == ilo) {

                guard3 = true;

                exitg2 = true;

              } else {

                cartesian_waypoints_planner_B.jp1 =

                  ((cartesian_waypoints_planner_B.j_kt - 1) << 2) +

                  cartesian_waypoints_planner_B.j_kt;

                if (fabs(A[cartesian_waypoints_planner_B.jp1].re) + fabs

                    (A[cartesian_waypoints_planner_B.jp1].im) <=

                    cartesian_waypoints_planner_B.b_atol) {

                  A[cartesian_waypoints_planner_B.jp1].re = 0.0;

                  A[cartesian_waypoints_planner_B.jp1].im = 0.0;

                  guard3 = true;

                  exitg2 = true;

                } else {

                  cartesian_waypoints_planner_B.j_kt--;

                  guard3 = false;

                }

              }

            }



            if (guard3) {

              cartesian_waypoints_planner_B.ifirst =

                cartesian_waypoints_planner_B.j_kt + 1;

              goto70 = true;

            }

          }

        }



        if ((!goto60) && (!goto70)) {

          alpha1[0].re = (rtNaN);

          alpha1[0].im = 0.0;

          beta1[0].re = (rtNaN);

          beta1[0].im = 0.0;

          alpha1[1].re = (rtNaN);

          alpha1[1].im = 0.0;

          beta1[1].re = (rtNaN);

          beta1[1].im = 0.0;

          alpha1[2].re = (rtNaN);

          alpha1[2].im = 0.0;

          beta1[2].re = (rtNaN);

          beta1[2].im = 0.0;

          alpha1[3].re = (rtNaN);

          alpha1[3].im = 0.0;

          beta1[3].re = (rtNaN);

          beta1[3].im = 0.0;

          for (cartesian_waypoints_planner_B.jp1 = 0;

               cartesian_waypoints_planner_B.jp1 < 16;

               cartesian_waypoints_planner_B.jp1++) {

            Z[cartesian_waypoints_planner_B.jp1].re = (rtNaN);

            Z[cartesian_waypoints_planner_B.jp1].im = 0.0;

          }



          *info = 1;

          exitg1 = 1;

        } else if (goto60) {

          goto60 = false;

          alpha1[cartesian_waypoints_planner_B.ilast] = A

            [(cartesian_waypoints_planner_B.ilast << 2) +

            cartesian_waypoints_planner_B.ilast];

          cartesian_waypoints_planner_B.ilast =

            cartesian_waypoints_planner_B.ilastm1;

          cartesian_waypoints_planner_B.ilastm1--;

          if (cartesian_waypoints_planner_B.ilast + 1 < ilo) {

            failed = false;

            guard2 = true;

            exitg1 = 1;

          } else {

            cartesian_waypoints_planner_B.iiter = 0;

            cartesian_waypoints_planner_B.eshift_re = 0.0;

            cartesian_waypoints_planner_B.eshift_im = 0.0;

            cartesian_waypoints_planner_B.jiter++;

          }

        } else {

          if (goto70) {

            goto70 = false;

            cartesian_waypoints_planner_B.iiter++;

            if (cartesian_waypoints_planner_B.iiter - div_nzp_s32

                (cartesian_waypoints_planner_B.iiter, 10) * 10 != 0) {

              cartesian_waypoints_planner_B.j_kt =

                (cartesian_waypoints_planner_B.ilastm1 << 2) +

                cartesian_waypoints_planner_B.ilastm1;

              cartesian_waypoints_planner_B.ar =

                A[cartesian_waypoints_planner_B.j_kt].re *

                cartesian_waypoints_planner_B.anorm;

              cartesian_waypoints_planner_B.ai =

                A[cartesian_waypoints_planner_B.j_kt].im *

                cartesian_waypoints_planner_B.anorm;

              if (cartesian_waypoints_planner_B.ai == 0.0) {

                cartesian_waypoints_planner_B.shift_re =

                  cartesian_waypoints_planner_B.ar / 0.5;

                cartesian_waypoints_planner_B.shift_im = 0.0;

              } else if (cartesian_waypoints_planner_B.ar == 0.0) {

                cartesian_waypoints_planner_B.shift_re = 0.0;

                cartesian_waypoints_planner_B.shift_im =

                  cartesian_waypoints_planner_B.ai / 0.5;

              } else {

                cartesian_waypoints_planner_B.shift_re =

                  cartesian_waypoints_planner_B.ar / 0.5;

                cartesian_waypoints_planner_B.shift_im =

                  cartesian_waypoints_planner_B.ai / 0.5;

              }



              cartesian_waypoints_planner_B.j_kt =

                (cartesian_waypoints_planner_B.ilast << 2) +

                cartesian_waypoints_planner_B.ilast;

              cartesian_waypoints_planner_B.ar =

                A[cartesian_waypoints_planner_B.j_kt].re *

                cartesian_waypoints_planner_B.anorm;

              cartesian_waypoints_planner_B.ai =

                A[cartesian_waypoints_planner_B.j_kt].im *

                cartesian_waypoints_planner_B.anorm;

              if (cartesian_waypoints_planner_B.ai == 0.0) {

                cartesian_waypoints_planner_B.ad22.re =

                  cartesian_waypoints_planner_B.ar / 0.5;

                cartesian_waypoints_planner_B.ad22.im = 0.0;

              } else if (cartesian_waypoints_planner_B.ar == 0.0) {

                cartesian_waypoints_planner_B.ad22.re = 0.0;

                cartesian_waypoints_planner_B.ad22.im =

                  cartesian_waypoints_planner_B.ai / 0.5;

              } else {

                cartesian_waypoints_planner_B.ad22.re =

                  cartesian_waypoints_planner_B.ar / 0.5;

                cartesian_waypoints_planner_B.ad22.im =

                  cartesian_waypoints_planner_B.ai / 0.5;

              }



              cartesian_waypoints_planner_B.t1_re =

                (cartesian_waypoints_planner_B.shift_re +

                 cartesian_waypoints_planner_B.ad22.re) * 0.5;

              cartesian_waypoints_planner_B.t1_im =

                (cartesian_waypoints_planner_B.shift_im +

                 cartesian_waypoints_planner_B.ad22.im) * 0.5;

              cartesian_waypoints_planner_B.j_kt =

                (cartesian_waypoints_planner_B.ilast << 2) +

                cartesian_waypoints_planner_B.ilastm1;

              cartesian_waypoints_planner_B.ar =

                A[cartesian_waypoints_planner_B.j_kt].re *

                cartesian_waypoints_planner_B.anorm;

              cartesian_waypoints_planner_B.ai =

                A[cartesian_waypoints_planner_B.j_kt].im *

                cartesian_waypoints_planner_B.anorm;

              if (cartesian_waypoints_planner_B.ai == 0.0) {

                cartesian_waypoints_planner_B.absxr =

                  cartesian_waypoints_planner_B.ar / 0.5;

                cartesian_waypoints_planner_B.absxi = 0.0;

              } else if (cartesian_waypoints_planner_B.ar == 0.0) {

                cartesian_waypoints_planner_B.absxr = 0.0;

                cartesian_waypoints_planner_B.absxi =

                  cartesian_waypoints_planner_B.ai / 0.5;

              } else {

                cartesian_waypoints_planner_B.absxr =

                  cartesian_waypoints_planner_B.ar / 0.5;

                cartesian_waypoints_planner_B.absxi =

                  cartesian_waypoints_planner_B.ai / 0.5;

              }



              cartesian_waypoints_planner_B.j_kt =

                (cartesian_waypoints_planner_B.ilastm1 << 2) +

                cartesian_waypoints_planner_B.ilast;

              cartesian_waypoints_planner_B.ar =

                A[cartesian_waypoints_planner_B.j_kt].re *

                cartesian_waypoints_planner_B.anorm;

              cartesian_waypoints_planner_B.ai =

                A[cartesian_waypoints_planner_B.j_kt].im *

                cartesian_waypoints_planner_B.anorm;

              if (cartesian_waypoints_planner_B.ai == 0.0) {

                cartesian_waypoints_planner_B.ar /= 0.5;

                cartesian_waypoints_planner_B.ai = 0.0;

              } else if (cartesian_waypoints_planner_B.ar == 0.0) {

                cartesian_waypoints_planner_B.ar = 0.0;

                cartesian_waypoints_planner_B.ai /= 0.5;

              } else {

                cartesian_waypoints_planner_B.ar /= 0.5;

                cartesian_waypoints_planner_B.ai /= 0.5;

              }



              cartesian_waypoints_planner_B.shift_im_f =

                cartesian_waypoints_planner_B.shift_re *

                cartesian_waypoints_planner_B.ad22.im +

                cartesian_waypoints_planner_B.shift_im *

                cartesian_waypoints_planner_B.ad22.re;

              cartesian_waypoints_planner_B.shift_re =

                ((cartesian_waypoints_planner_B.t1_re *

                  cartesian_waypoints_planner_B.t1_re -

                  cartesian_waypoints_planner_B.t1_im *

                  cartesian_waypoints_planner_B.t1_im) +

                 (cartesian_waypoints_planner_B.absxr *

                  cartesian_waypoints_planner_B.ar -

                  cartesian_waypoints_planner_B.absxi *

                  cartesian_waypoints_planner_B.ai)) -

                (cartesian_waypoints_planner_B.shift_re *

                 cartesian_waypoints_planner_B.ad22.re -

                 cartesian_waypoints_planner_B.shift_im *

                 cartesian_waypoints_planner_B.ad22.im);

              cartesian_waypoints_planner_B.shift_im =

                cartesian_waypoints_planner_B.t1_re *

                cartesian_waypoints_planner_B.t1_im;

              cartesian_waypoints_planner_B.shift_im =

                ((cartesian_waypoints_planner_B.shift_im +

                  cartesian_waypoints_planner_B.shift_im) +

                 (cartesian_waypoints_planner_B.absxr *

                  cartesian_waypoints_planner_B.ai +

                  cartesian_waypoints_planner_B.absxi *

                  cartesian_waypoints_planner_B.ar)) -

                cartesian_waypoints_planner_B.shift_im_f;

              if (cartesian_waypoints_planner_B.shift_im == 0.0) {

                if (cartesian_waypoints_planner_B.shift_re < 0.0) {

                  cartesian_waypoints_planner_B.absxr = 0.0;

                  cartesian_waypoints_planner_B.absxi = sqrt

                    (-cartesian_waypoints_planner_B.shift_re);

                } else {

                  cartesian_waypoints_planner_B.absxr = sqrt

                    (cartesian_waypoints_planner_B.shift_re);

                  cartesian_waypoints_planner_B.absxi = 0.0;

                }

              } else if (cartesian_waypoints_planner_B.shift_re == 0.0) {

                if (cartesian_waypoints_planner_B.shift_im < 0.0) {

                  cartesian_waypoints_planner_B.absxr = sqrt

                    (-cartesian_waypoints_planner_B.shift_im / 2.0);

                  cartesian_waypoints_planner_B.absxi =

                    -cartesian_waypoints_planner_B.absxr;

                } else {

                  cartesian_waypoints_planner_B.absxr = sqrt

                    (cartesian_waypoints_planner_B.shift_im / 2.0);

                  cartesian_waypoints_planner_B.absxi =

                    cartesian_waypoints_planner_B.absxr;

                }

              } else if (rtIsNaN(cartesian_waypoints_planner_B.shift_re)) {

                cartesian_waypoints_planner_B.absxr =

                  cartesian_waypoints_planner_B.shift_re;

                cartesian_waypoints_planner_B.absxi =

                  cartesian_waypoints_planner_B.shift_re;

              } else if (rtIsNaN(cartesian_waypoints_planner_B.shift_im)) {

                cartesian_waypoints_planner_B.absxr =

                  cartesian_waypoints_planner_B.shift_im;

                cartesian_waypoints_planner_B.absxi =

                  cartesian_waypoints_planner_B.shift_im;

              } else if (rtIsInf(cartesian_waypoints_planner_B.shift_im)) {

                cartesian_waypoints_planner_B.absxr = fabs

                  (cartesian_waypoints_planner_B.shift_im);

                cartesian_waypoints_planner_B.absxi =

                  cartesian_waypoints_planner_B.shift_im;

              } else if (rtIsInf(cartesian_waypoints_planner_B.shift_re)) {

                if (cartesian_waypoints_planner_B.shift_re < 0.0) {

                  cartesian_waypoints_planner_B.absxr = 0.0;

                  cartesian_waypoints_planner_B.absxi =

                    cartesian_waypoints_planner_B.shift_im *

                    -cartesian_waypoints_planner_B.shift_re;

                } else {

                  cartesian_waypoints_planner_B.absxr =

                    cartesian_waypoints_planner_B.shift_re;

                  cartesian_waypoints_planner_B.absxi = 0.0;

                }

              } else {

                cartesian_waypoints_planner_B.absxr = fabs

                  (cartesian_waypoints_planner_B.shift_re);

                cartesian_waypoints_planner_B.absxi = fabs

                  (cartesian_waypoints_planner_B.shift_im);

                if ((cartesian_waypoints_planner_B.absxr >

                     4.4942328371557893E+307) ||

                    (cartesian_waypoints_planner_B.absxi >

                     4.4942328371557893E+307)) {

                  cartesian_waypoints_planner_B.absxr *= 0.5;

                  cartesian_waypoints_planner_B.absxi *= 0.5;

                  cartesian_waypoints_planner_B.absxi =

                    cartesian_waypoin_rt_hypotd_snf

                    (cartesian_waypoints_planner_B.absxr,

                     cartesian_waypoints_planner_B.absxi);

                  if (cartesian_waypoints_planner_B.absxi >

                      cartesian_waypoints_planner_B.absxr) {

                    cartesian_waypoints_planner_B.absxr = sqrt

                      (cartesian_waypoints_planner_B.absxr /

                       cartesian_waypoints_planner_B.absxi + 1.0) * sqrt

                      (cartesian_waypoints_planner_B.absxi);

                  } else {

                    cartesian_waypoints_planner_B.absxr = sqrt

                      (cartesian_waypoints_planner_B.absxi) * 1.4142135623730951;

                  }

                } else {

                  cartesian_waypoints_planner_B.absxr = sqrt

                    ((cartesian_waypoin_rt_hypotd_snf

                      (cartesian_waypoints_planner_B.absxr,

                       cartesian_waypoints_planner_B.absxi) +

                      cartesian_waypoints_planner_B.absxr) * 0.5);

                }



                if (cartesian_waypoints_planner_B.shift_re > 0.0) {

                  cartesian_waypoints_planner_B.absxi =

                    cartesian_waypoints_planner_B.shift_im /

                    cartesian_waypoints_planner_B.absxr * 0.5;

                } else {

                  if (cartesian_waypoints_planner_B.shift_im < 0.0) {

                    cartesian_waypoints_planner_B.absxi =

                      -cartesian_waypoints_planner_B.absxr;

                  } else {

                    cartesian_waypoints_planner_B.absxi =

                      cartesian_waypoints_planner_B.absxr;

                  }



                  cartesian_waypoints_planner_B.absxr =

                    cartesian_waypoints_planner_B.shift_im /

                    cartesian_waypoints_planner_B.absxi * 0.5;

                }

              }



              if ((cartesian_waypoints_planner_B.t1_re -

                   cartesian_waypoints_planner_B.ad22.re) *

                  cartesian_waypoints_planner_B.absxr +

                  (cartesian_waypoints_planner_B.t1_im -

                   cartesian_waypoints_planner_B.ad22.im) *

                  cartesian_waypoints_planner_B.absxi <= 0.0) {

                cartesian_waypoints_planner_B.shift_re =

                  cartesian_waypoints_planner_B.t1_re +

                  cartesian_waypoints_planner_B.absxr;

                cartesian_waypoints_planner_B.shift_im =

                  cartesian_waypoints_planner_B.t1_im +

                  cartesian_waypoints_planner_B.absxi;

              } else {

                cartesian_waypoints_planner_B.shift_re =

                  cartesian_waypoints_planner_B.t1_re -

                  cartesian_waypoints_planner_B.absxr;

                cartesian_waypoints_planner_B.shift_im =

                  cartesian_waypoints_planner_B.t1_im -

                  cartesian_waypoints_planner_B.absxi;

              }

            } else {

              cartesian_waypoints_planner_B.j_kt =

                (cartesian_waypoints_planner_B.ilastm1 << 2) +

                cartesian_waypoints_planner_B.ilast;

              cartesian_waypoints_planner_B.ar =

                A[cartesian_waypoints_planner_B.j_kt].re *

                cartesian_waypoints_planner_B.anorm;

              cartesian_waypoints_planner_B.ai =

                A[cartesian_waypoints_planner_B.j_kt].im *

                cartesian_waypoints_planner_B.anorm;

              if (cartesian_waypoints_planner_B.ai == 0.0) {

                cartesian_waypoints_planner_B.absxr =

                  cartesian_waypoints_planner_B.ar / 0.5;

                cartesian_waypoints_planner_B.absxi = 0.0;

              } else if (cartesian_waypoints_planner_B.ar == 0.0) {

                cartesian_waypoints_planner_B.absxr = 0.0;

                cartesian_waypoints_planner_B.absxi =

                  cartesian_waypoints_planner_B.ai / 0.5;

              } else {

                cartesian_waypoints_planner_B.absxr =

                  cartesian_waypoints_planner_B.ar / 0.5;

                cartesian_waypoints_planner_B.absxi =

                  cartesian_waypoints_planner_B.ai / 0.5;

              }



              cartesian_waypoints_planner_B.eshift_re +=

                cartesian_waypoints_planner_B.absxr;

              cartesian_waypoints_planner_B.eshift_im +=

                cartesian_waypoints_planner_B.absxi;

              cartesian_waypoints_planner_B.shift_re =

                cartesian_waypoints_planner_B.eshift_re;

              cartesian_waypoints_planner_B.shift_im =

                cartesian_waypoints_planner_B.eshift_im;

            }



            cartesian_waypoints_planner_B.j_kt =

              cartesian_waypoints_planner_B.ilastm1;

            cartesian_waypoints_planner_B.jp1 =

              cartesian_waypoints_planner_B.ilastm1 + 1;

            exitg2 = false;

            while ((!exitg2) && (cartesian_waypoints_planner_B.j_kt + 1 >

                                 cartesian_waypoints_planner_B.ifirst)) {

              cartesian_waypoints_planner_B.istart =

                cartesian_waypoints_planner_B.j_kt + 1;

              cartesian_waypoints_planner_B.ctemp_tmp_tmp =

                cartesian_waypoints_planner_B.j_kt << 2;

              cartesian_waypoints_planner_B.ctemp_tmp =

                cartesian_waypoints_planner_B.ctemp_tmp_tmp +

                cartesian_waypoints_planner_B.j_kt;

              cartesian_waypoints_planner_B.ctemp.re =

                A[cartesian_waypoints_planner_B.ctemp_tmp].re *

                cartesian_waypoints_planner_B.anorm -

                cartesian_waypoints_planner_B.shift_re * 0.5;

              cartesian_waypoints_planner_B.ctemp.im =

                A[cartesian_waypoints_planner_B.ctemp_tmp].im *

                cartesian_waypoints_planner_B.anorm -

                cartesian_waypoints_planner_B.shift_im * 0.5;

              cartesian_waypoints_planner_B.t1_re = fabs

                (cartesian_waypoints_planner_B.ctemp.re) + fabs

                (cartesian_waypoints_planner_B.ctemp.im);

              cartesian_waypoints_planner_B.jp1 +=

                cartesian_waypoints_planner_B.ctemp_tmp_tmp;

              cartesian_waypoints_planner_B.t1_im = (fabs

                (A[cartesian_waypoints_planner_B.jp1].re) + fabs

                (A[cartesian_waypoints_planner_B.jp1].im)) *

                cartesian_waypoints_planner_B.anorm;

              cartesian_waypoints_planner_B.absxr =

                cartesian_waypoints_planner_B.t1_re;

              if (cartesian_waypoints_planner_B.t1_im >

                  cartesian_waypoints_planner_B.t1_re) {

                cartesian_waypoints_planner_B.absxr =

                  cartesian_waypoints_planner_B.t1_im;

              }



              if ((cartesian_waypoints_planner_B.absxr < 1.0) &&

                  (cartesian_waypoints_planner_B.absxr != 0.0)) {

                cartesian_waypoints_planner_B.t1_re /=

                  cartesian_waypoints_planner_B.absxr;

                cartesian_waypoints_planner_B.t1_im /=

                  cartesian_waypoints_planner_B.absxr;

              }



              cartesian_waypoints_planner_B.jp1 =

                ((cartesian_waypoints_planner_B.j_kt - 1) << 2) +

                cartesian_waypoints_planner_B.j_kt;

              if ((fabs(A[cartesian_waypoints_planner_B.jp1].re) + fabs

                   (A[cartesian_waypoints_planner_B.jp1].im)) *

                  cartesian_waypoints_planner_B.t1_im <=

                  cartesian_waypoints_planner_B.t1_re *

                  cartesian_waypoints_planner_B.b_atol) {

                goto90 = true;

                exitg2 = true;

              } else {

                cartesian_waypoints_planner_B.jp1 =

                  cartesian_waypoints_planner_B.j_kt;

                cartesian_waypoints_planner_B.j_kt--;

              }

            }



            if (!goto90) {

              cartesian_waypoints_planner_B.istart =

                cartesian_waypoints_planner_B.ifirst;

              cartesian_waypoints_planner_B.ctemp_tmp =

                (((cartesian_waypoints_planner_B.ifirst - 1) << 2) +

                 cartesian_waypoints_planner_B.ifirst) - 1;

              cartesian_waypoints_planner_B.ctemp.re =

                A[cartesian_waypoints_planner_B.ctemp_tmp].re *

                cartesian_waypoints_planner_B.anorm -

                cartesian_waypoints_planner_B.shift_re * 0.5;

              cartesian_waypoints_planner_B.ctemp.im =

                A[cartesian_waypoints_planner_B.ctemp_tmp].im *

                cartesian_waypoints_planner_B.anorm -

                cartesian_waypoints_planner_B.shift_im * 0.5;

            }



            goto90 = false;

            cartesian_waypoints_planner_B.j_kt =

              ((cartesian_waypoints_planner_B.istart - 1) << 2) +

              cartesian_waypoints_planner_B.istart;

            cartesian_waypoints_planner_B.ascale.re =

              A[cartesian_waypoints_planner_B.j_kt].re *

              cartesian_waypoints_planner_B.anorm;

            cartesian_waypoints_planner_B.ascale.im =

              A[cartesian_waypoints_planner_B.j_kt].im *

              cartesian_waypoints_planner_B.anorm;

            cartesian_waypoints_p_xzlartg_i(cartesian_waypoints_planner_B.ctemp,

              cartesian_waypoints_planner_B.ascale,

              &cartesian_waypoints_planner_B.t1_re,

              &cartesian_waypoints_planner_B.ad22);

            cartesian_waypoints_planner_B.j_kt =

              cartesian_waypoints_planner_B.istart;

            cartesian_waypoints_planner_B.jp1 =

              cartesian_waypoints_planner_B.istart - 2;

            while (cartesian_waypoints_planner_B.j_kt <

                   cartesian_waypoints_planner_B.ilast + 1) {

              if (cartesian_waypoints_planner_B.j_kt >

                  cartesian_waypoints_planner_B.istart) {

                cartesian_waypoints_pla_xzlartg(A

                  [(cartesian_waypoints_planner_B.j_kt +

                    (cartesian_waypoints_planner_B.jp1 << 2)) - 1],

                  A[cartesian_waypoints_planner_B.j_kt +

                  (cartesian_waypoints_planner_B.jp1 << 2)],

                  &cartesian_waypoints_planner_B.t1_re,

                  &cartesian_waypoints_planner_B.ad22, &A

                  [(cartesian_waypoints_planner_B.j_kt +

                    (cartesian_waypoints_planner_B.jp1 << 2)) - 1]);

                cartesian_waypoints_planner_B.jp1 =

                  cartesian_waypoints_planner_B.j_kt +

                  (cartesian_waypoints_planner_B.jp1 << 2);

                A[cartesian_waypoints_planner_B.jp1].re = 0.0;

                A[cartesian_waypoints_planner_B.jp1].im = 0.0;

              }



              cartesian_waypoints_planner_B.ctemp_tmp =

                cartesian_waypoints_planner_B.j_kt - 1;

              while (cartesian_waypoints_planner_B.ctemp_tmp + 1 < 5) {

                cartesian_waypoints_planner_B.jp1 =

                  (cartesian_waypoints_planner_B.ctemp_tmp << 2) +

                  cartesian_waypoints_planner_B.j_kt;

                cartesian_waypoints_planner_B.ctemp_tmp_tmp =

                  cartesian_waypoints_planner_B.jp1 - 1;

                cartesian_waypoints_planner_B.shift_re =

                  A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re *

                  cartesian_waypoints_planner_B.t1_re +

                  (A[cartesian_waypoints_planner_B.jp1].re *

                   cartesian_waypoints_planner_B.ad22.re -

                   A[cartesian_waypoints_planner_B.jp1].im *

                   cartesian_waypoints_planner_B.ad22.im);

                cartesian_waypoints_planner_B.shift_im =

                  A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im *

                  cartesian_waypoints_planner_B.t1_re +

                  (A[cartesian_waypoints_planner_B.jp1].im *

                   cartesian_waypoints_planner_B.ad22.re +

                   A[cartesian_waypoints_planner_B.jp1].re *

                   cartesian_waypoints_planner_B.ad22.im);

                cartesian_waypoints_planner_B.t1_im =

                  A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im;

                cartesian_waypoints_planner_B.absxr =

                  A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re;

                A[cartesian_waypoints_planner_B.jp1].re =

                  A[cartesian_waypoints_planner_B.jp1].re *

                  cartesian_waypoints_planner_B.t1_re -

                  (A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re *

                   cartesian_waypoints_planner_B.ad22.re +

                   A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im *

                   cartesian_waypoints_planner_B.ad22.im);

                A[cartesian_waypoints_planner_B.jp1].im =

                  A[cartesian_waypoints_planner_B.jp1].im *

                  cartesian_waypoints_planner_B.t1_re -

                  (cartesian_waypoints_planner_B.ad22.re *

                   cartesian_waypoints_planner_B.t1_im -

                   cartesian_waypoints_planner_B.ad22.im *

                   cartesian_waypoints_planner_B.absxr);

                A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re =

                  cartesian_waypoints_planner_B.shift_re;

                A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im =

                  cartesian_waypoints_planner_B.shift_im;

                cartesian_waypoints_planner_B.ctemp_tmp++;

              }



              cartesian_waypoints_planner_B.ad22.re =

                -cartesian_waypoints_planner_B.ad22.re;

              cartesian_waypoints_planner_B.ad22.im =

                -cartesian_waypoints_planner_B.ad22.im;

              cartesian_waypoints_planner_B.ctemp_tmp =

                cartesian_waypoints_planner_B.j_kt;

              if (cartesian_waypoints_planner_B.ilast + 1 <

                  cartesian_waypoints_planner_B.j_kt + 2) {

                cartesian_waypoints_planner_B.ctemp_tmp =

                  cartesian_waypoints_planner_B.ilast - 1;

              }



              cartesian_waypoints_planner_B.i_a0 = 0;

              while (cartesian_waypoints_planner_B.i_a0 + 1 <=

                     cartesian_waypoints_planner_B.ctemp_tmp + 2) {

                cartesian_waypoints_planner_B.jp1 =

                  ((cartesian_waypoints_planner_B.j_kt - 1) << 2) +

                  cartesian_waypoints_planner_B.i_a0;

                cartesian_waypoints_planner_B.ctemp_tmp_tmp =

                  (cartesian_waypoints_planner_B.j_kt << 2) +

                  cartesian_waypoints_planner_B.i_a0;

                cartesian_waypoints_planner_B.shift_re =

                  (A[cartesian_waypoints_planner_B.jp1].re *

                   cartesian_waypoints_planner_B.ad22.re -

                   A[cartesian_waypoints_planner_B.jp1].im *

                   cartesian_waypoints_planner_B.ad22.im) +

                  A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re *

                  cartesian_waypoints_planner_B.t1_re;

                cartesian_waypoints_planner_B.shift_im =

                  (A[cartesian_waypoints_planner_B.jp1].im *

                   cartesian_waypoints_planner_B.ad22.re +

                   A[cartesian_waypoints_planner_B.jp1].re *

                   cartesian_waypoints_planner_B.ad22.im) +

                  A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im *

                  cartesian_waypoints_planner_B.t1_re;

                cartesian_waypoints_planner_B.t1_im =

                  A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im;

                cartesian_waypoints_planner_B.absxr =

                  A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re;

                A[cartesian_waypoints_planner_B.jp1].re =

                  A[cartesian_waypoints_planner_B.jp1].re *

                  cartesian_waypoints_planner_B.t1_re -

                  (A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re *

                   cartesian_waypoints_planner_B.ad22.re +

                   A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im *

                   cartesian_waypoints_planner_B.ad22.im);

                A[cartesian_waypoints_planner_B.jp1].im =

                  A[cartesian_waypoints_planner_B.jp1].im *

                  cartesian_waypoints_planner_B.t1_re -

                  (cartesian_waypoints_planner_B.ad22.re *

                   cartesian_waypoints_planner_B.t1_im -

                   cartesian_waypoints_planner_B.ad22.im *

                   cartesian_waypoints_planner_B.absxr);

                A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re =

                  cartesian_waypoints_planner_B.shift_re;

                A[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im =

                  cartesian_waypoints_planner_B.shift_im;

                cartesian_waypoints_planner_B.i_a0++;

              }



              cartesian_waypoints_planner_B.jp1 =

                (cartesian_waypoints_planner_B.j_kt - 1) << 2;

              cartesian_waypoints_planner_B.ctemp_tmp_tmp =

                cartesian_waypoints_planner_B.j_kt << 2;

              cartesian_waypoints_planner_B.shift_re =

                (Z[cartesian_waypoints_planner_B.jp1].re *

                 cartesian_waypoints_planner_B.ad22.re -

                 Z[cartesian_waypoints_planner_B.jp1].im *

                 cartesian_waypoints_planner_B.ad22.im) +

                Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re *

                cartesian_waypoints_planner_B.t1_re;

              cartesian_waypoints_planner_B.shift_im =

                (Z[cartesian_waypoints_planner_B.jp1].im *

                 cartesian_waypoints_planner_B.ad22.re +

                 Z[cartesian_waypoints_planner_B.jp1].re *

                 cartesian_waypoints_planner_B.ad22.im) +

                Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im *

                cartesian_waypoints_planner_B.t1_re;

              cartesian_waypoints_planner_B.t1_im =

                Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im;

              cartesian_waypoints_planner_B.absxr =

                Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re;

              Z[cartesian_waypoints_planner_B.jp1].re =

                Z[cartesian_waypoints_planner_B.jp1].re *

                cartesian_waypoints_planner_B.t1_re -

                (Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re *

                 cartesian_waypoints_planner_B.ad22.re +

                 Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im *

                 cartesian_waypoints_planner_B.ad22.im);

              Z[cartesian_waypoints_planner_B.jp1].im =

                Z[cartesian_waypoints_planner_B.jp1].im *

                cartesian_waypoints_planner_B.t1_re -

                (cartesian_waypoints_planner_B.ad22.re *

                 cartesian_waypoints_planner_B.t1_im -

                 cartesian_waypoints_planner_B.ad22.im *

                 cartesian_waypoints_planner_B.absxr);

              Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re =

                cartesian_waypoints_planner_B.shift_re;

              Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im =

                cartesian_waypoints_planner_B.shift_im;

              cartesian_waypoints_planner_B.ctemp_tmp =

                cartesian_waypoints_planner_B.jp1 + 1;

              cartesian_waypoints_planner_B.i_a0 =

                cartesian_waypoints_planner_B.ctemp_tmp_tmp + 1;

              cartesian_waypoints_planner_B.shift_re =

                (Z[cartesian_waypoints_planner_B.ctemp_tmp].re *

                 cartesian_waypoints_planner_B.ad22.re -

                 Z[cartesian_waypoints_planner_B.ctemp_tmp].im *

                 cartesian_waypoints_planner_B.ad22.im) +

                Z[cartesian_waypoints_planner_B.i_a0].re *

                cartesian_waypoints_planner_B.t1_re;

              cartesian_waypoints_planner_B.shift_im =

                (Z[cartesian_waypoints_planner_B.ctemp_tmp].im *

                 cartesian_waypoints_planner_B.ad22.re +

                 Z[cartesian_waypoints_planner_B.ctemp_tmp].re *

                 cartesian_waypoints_planner_B.ad22.im) +

                Z[cartesian_waypoints_planner_B.i_a0].im *

                cartesian_waypoints_planner_B.t1_re;

              cartesian_waypoints_planner_B.t1_im =

                Z[cartesian_waypoints_planner_B.i_a0].im;

              cartesian_waypoints_planner_B.absxr =

                Z[cartesian_waypoints_planner_B.i_a0].re;

              Z[cartesian_waypoints_planner_B.ctemp_tmp].re =

                Z[cartesian_waypoints_planner_B.ctemp_tmp].re *

                cartesian_waypoints_planner_B.t1_re -

                (Z[cartesian_waypoints_planner_B.i_a0].re *

                 cartesian_waypoints_planner_B.ad22.re +

                 Z[cartesian_waypoints_planner_B.i_a0].im *

                 cartesian_waypoints_planner_B.ad22.im);

              Z[cartesian_waypoints_planner_B.ctemp_tmp].im =

                Z[cartesian_waypoints_planner_B.ctemp_tmp].im *

                cartesian_waypoints_planner_B.t1_re -

                (cartesian_waypoints_planner_B.ad22.re *

                 cartesian_waypoints_planner_B.t1_im -

                 cartesian_waypoints_planner_B.ad22.im *

                 cartesian_waypoints_planner_B.absxr);

              Z[cartesian_waypoints_planner_B.i_a0].re =

                cartesian_waypoints_planner_B.shift_re;

              Z[cartesian_waypoints_planner_B.i_a0].im =

                cartesian_waypoints_planner_B.shift_im;

              cartesian_waypoints_planner_B.ctemp_tmp =

                cartesian_waypoints_planner_B.jp1 + 2;

              cartesian_waypoints_planner_B.i_a0 =

                cartesian_waypoints_planner_B.ctemp_tmp_tmp + 2;

              cartesian_waypoints_planner_B.shift_re =

                (Z[cartesian_waypoints_planner_B.ctemp_tmp].re *

                 cartesian_waypoints_planner_B.ad22.re -

                 Z[cartesian_waypoints_planner_B.ctemp_tmp].im *

                 cartesian_waypoints_planner_B.ad22.im) +

                Z[cartesian_waypoints_planner_B.i_a0].re *

                cartesian_waypoints_planner_B.t1_re;

              cartesian_waypoints_planner_B.shift_im =

                (Z[cartesian_waypoints_planner_B.ctemp_tmp].im *

                 cartesian_waypoints_planner_B.ad22.re +

                 Z[cartesian_waypoints_planner_B.ctemp_tmp].re *

                 cartesian_waypoints_planner_B.ad22.im) +

                Z[cartesian_waypoints_planner_B.i_a0].im *

                cartesian_waypoints_planner_B.t1_re;

              cartesian_waypoints_planner_B.t1_im =

                Z[cartesian_waypoints_planner_B.i_a0].im;

              cartesian_waypoints_planner_B.absxr =

                Z[cartesian_waypoints_planner_B.i_a0].re;

              Z[cartesian_waypoints_planner_B.ctemp_tmp].re =

                Z[cartesian_waypoints_planner_B.ctemp_tmp].re *

                cartesian_waypoints_planner_B.t1_re -

                (Z[cartesian_waypoints_planner_B.i_a0].re *

                 cartesian_waypoints_planner_B.ad22.re +

                 Z[cartesian_waypoints_planner_B.i_a0].im *

                 cartesian_waypoints_planner_B.ad22.im);

              Z[cartesian_waypoints_planner_B.ctemp_tmp].im =

                Z[cartesian_waypoints_planner_B.ctemp_tmp].im *

                cartesian_waypoints_planner_B.t1_re -

                (cartesian_waypoints_planner_B.ad22.re *

                 cartesian_waypoints_planner_B.t1_im -

                 cartesian_waypoints_planner_B.ad22.im *

                 cartesian_waypoints_planner_B.absxr);

              Z[cartesian_waypoints_planner_B.i_a0].re =

                cartesian_waypoints_planner_B.shift_re;

              Z[cartesian_waypoints_planner_B.i_a0].im =

                cartesian_waypoints_planner_B.shift_im;

              cartesian_waypoints_planner_B.jp1 += 3;

              cartesian_waypoints_planner_B.ctemp_tmp_tmp += 3;

              cartesian_waypoints_planner_B.shift_re =

                (Z[cartesian_waypoints_planner_B.jp1].re *

                 cartesian_waypoints_planner_B.ad22.re -

                 Z[cartesian_waypoints_planner_B.jp1].im *

                 cartesian_waypoints_planner_B.ad22.im) +

                Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re *

                cartesian_waypoints_planner_B.t1_re;

              cartesian_waypoints_planner_B.shift_im =

                (Z[cartesian_waypoints_planner_B.jp1].im *

                 cartesian_waypoints_planner_B.ad22.re +

                 Z[cartesian_waypoints_planner_B.jp1].re *

                 cartesian_waypoints_planner_B.ad22.im) +

                Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im *

                cartesian_waypoints_planner_B.t1_re;

              cartesian_waypoints_planner_B.t1_im =

                Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im;

              cartesian_waypoints_planner_B.absxr =

                Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re;

              Z[cartesian_waypoints_planner_B.jp1].re =

                Z[cartesian_waypoints_planner_B.jp1].re *

                cartesian_waypoints_planner_B.t1_re -

                (Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re *

                 cartesian_waypoints_planner_B.ad22.re +

                 Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im *

                 cartesian_waypoints_planner_B.ad22.im);

              Z[cartesian_waypoints_planner_B.jp1].im =

                Z[cartesian_waypoints_planner_B.jp1].im *

                cartesian_waypoints_planner_B.t1_re -

                (cartesian_waypoints_planner_B.ad22.re *

                 cartesian_waypoints_planner_B.t1_im -

                 cartesian_waypoints_planner_B.ad22.im *

                 cartesian_waypoints_planner_B.absxr);

              Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].re =

                cartesian_waypoints_planner_B.shift_re;

              Z[cartesian_waypoints_planner_B.ctemp_tmp_tmp].im =

                cartesian_waypoints_planner_B.shift_im;

              cartesian_waypoints_planner_B.jp1 =

                cartesian_waypoints_planner_B.j_kt - 1;

              cartesian_waypoints_planner_B.j_kt++;

            }

          }



          cartesian_waypoints_planner_B.jiter++;

        }

      } else {

        guard2 = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  } else {

    guard1 = true;

  }



  if (guard2) {

    if (failed) {

      *info = cartesian_waypoints_planner_B.ilast + 1;

      cartesian_waypoints_planner_B.ifirst = 0;

      while (cartesian_waypoints_planner_B.ifirst <=

             cartesian_waypoints_planner_B.ilast) {

        alpha1[cartesian_waypoints_planner_B.ifirst].re = (rtNaN);

        alpha1[cartesian_waypoints_planner_B.ifirst].im = 0.0;

        beta1[cartesian_waypoints_planner_B.ifirst].re = (rtNaN);

        beta1[cartesian_waypoints_planner_B.ifirst].im = 0.0;

        cartesian_waypoints_planner_B.ifirst++;

      }



      for (cartesian_waypoints_planner_B.jp1 = 0;

           cartesian_waypoints_planner_B.jp1 < 16;

           cartesian_waypoints_planner_B.jp1++) {

        Z[cartesian_waypoints_planner_B.jp1].re = (rtNaN);

        Z[cartesian_waypoints_planner_B.jp1].im = 0.0;

      }

    } else {

      guard1 = true;

    }

  }



  if (guard1) {

    cartesian_waypoints_planner_B.ilast = 0;

    while (cartesian_waypoints_planner_B.ilast <= ilo - 2) {

      alpha1[cartesian_waypoints_planner_B.ilast] = A

        [(cartesian_waypoints_planner_B.ilast << 2) +

        cartesian_waypoints_planner_B.ilast];

      cartesian_waypoints_planner_B.ilast++;

    }

  }

}



// Function for MATLAB Function: '<S9>/MATLAB Function5'

static void cartesian_waypoints_pla_xztgevc(const creal_T A[16], creal_T V[16])

{

  creal_T work1[4];

  real_T rworka[4];

  real_T anorm;

  real_T ascale;

  real_T temp;

  real_T acoeff;

  real_T scale;

  real_T dmin;

  boolean_T lscalea;

  boolean_T lscaleb;

  int32_T i;

  int32_T c_j;

  int32_T e_jr;

  real_T f_y;

  real_T salpha_re;

  real_T salpha_im;

  real_T work2_idx_2_im;

  real_T work2_idx_3_re;

  real_T work2_idx_3_im;

  int32_T c_x_tmp;

  int32_T c_x_tmp_tmp;

  int32_T d_re_tmp;

  int32_T work2_idx_1_re_tmp;

  int32_T d_re_tmp_tmp;

  rworka[0] = 0.0;

  rworka[2] = 0.0;

  rworka[3] = 0.0;

  anorm = fabs(A[0].re) + fabs(A[0].im);

  rworka[1] = fabs(A[4].re) + fabs(A[4].im);

  ascale = (fabs(A[5].re) + fabs(A[5].im)) + rworka[1];

  if (ascale > anorm) {

    anorm = ascale;

  }



  for (i = 0; i < 2; i++) {

    rworka[2] += fabs(A[i + 8].re) + fabs(A[i + 8].im);

  }



  ascale = (fabs(A[10].re) + fabs(A[10].im)) + rworka[2];

  if (ascale > anorm) {

    anorm = ascale;

  }



  for (i = 0; i < 3; i++) {

    rworka[3] += fabs(A[i + 12].re) + fabs(A[i + 12].im);

  }



  ascale = (fabs(A[15].re) + fabs(A[15].im)) + rworka[3];

  if (ascale > anorm) {

    anorm = ascale;

  }



  ascale = anorm;

  if (2.2250738585072014E-308 > anorm) {

    ascale = 2.2250738585072014E-308;

  }



  ascale = 1.0 / ascale;

  for (i = 0; i < 4; i++) {

    c_x_tmp_tmp = (3 - i) << 2;

    c_x_tmp = (c_x_tmp_tmp - i) + 3;

    salpha_re = (fabs(A[c_x_tmp].re) + fabs(A[c_x_tmp].im)) * ascale;

    if (1.0 > salpha_re) {

      salpha_re = 1.0;

    }



    temp = 1.0 / salpha_re;

    salpha_re = A[c_x_tmp].re * temp * ascale;

    salpha_im = A[c_x_tmp].im * temp * ascale;

    acoeff = temp * ascale;

    lscalea = ((temp >= 2.2250738585072014E-308) && (acoeff <

                4.0083367200179456E-292));

    dmin = fabs(salpha_re) + fabs(salpha_im);

    lscaleb = ((dmin >= 2.2250738585072014E-308) && (dmin <

                4.0083367200179456E-292));

    scale = 1.0;

    if (lscalea) {

      scale = anorm;

      if (2.4948003869184E+291 < anorm) {

        scale = 2.4948003869184E+291;

      }



      scale *= 4.0083367200179456E-292 / temp;

    }



    if (lscaleb) {

      work2_idx_2_im = 4.0083367200179456E-292 / dmin;

      if (work2_idx_2_im > scale) {

        scale = work2_idx_2_im;

      }

    }



    if (lscalea || lscaleb) {

      work2_idx_2_im = acoeff;

      if (1.0 > acoeff) {

        work2_idx_2_im = 1.0;

      }



      if (dmin > work2_idx_2_im) {

        work2_idx_2_im = dmin;

      }



      dmin = 1.0 / (2.2250738585072014E-308 * work2_idx_2_im);

      if (dmin < scale) {

        scale = dmin;

      }



      if (lscalea) {

        acoeff = scale * temp * ascale;

      } else {

        acoeff *= scale;

      }



      salpha_re *= scale;

      salpha_im *= scale;

    }



    memset(&work1[0], 0, sizeof(creal_T) << 2U);

    work1[3 - i].re = 1.0;

    work1[3 - i].im = 0.0;

    dmin = 2.2204460492503131E-16 * acoeff * anorm;

    temp = (fabs(salpha_re) + fabs(salpha_im)) * 2.2204460492503131E-16;

    if (temp > dmin) {

      dmin = temp;

    }



    if (2.2250738585072014E-308 > dmin) {

      dmin = 2.2250738585072014E-308;

    }



    for (c_x_tmp = 0; c_x_tmp <= 2 - i; c_x_tmp++) {

      d_re_tmp = c_x_tmp_tmp + c_x_tmp;

      work1[c_x_tmp].re = A[d_re_tmp].re * acoeff;

      work1[c_x_tmp].im = A[d_re_tmp].im * acoeff;

    }



    work1[3 - i].re = 1.0;

    work1[3 - i].im = 0.0;

    c_x_tmp = static_cast<int32_T>(((-1.0 - ((-static_cast<real_T>(i) + 4.0) -

      1.0)) + 1.0) / -1.0);

    for (c_j = 0; c_j < c_x_tmp; c_j++) {

      work2_idx_1_re_tmp = 2 - (i + c_j);

      d_re_tmp_tmp = work2_idx_1_re_tmp << 2;

      d_re_tmp = d_re_tmp_tmp + work2_idx_1_re_tmp;

      work2_idx_3_re = A[d_re_tmp].re * acoeff - salpha_re;

      scale = A[d_re_tmp].im * acoeff - salpha_im;

      if (fabs(work2_idx_3_re) + fabs(scale) <= dmin) {

        work2_idx_3_re = dmin;

        scale = 0.0;

      }



      work2_idx_2_im = fabs(work2_idx_3_re);

      work2_idx_3_im = fabs(scale);

      temp = work2_idx_2_im + work2_idx_3_im;

      if (temp < 1.0) {

        f_y = fabs(work1[work2_idx_1_re_tmp].re) + fabs(work1[work2_idx_1_re_tmp]

          .im);

        if (f_y >= temp * 1.1235582092889474E+307) {

          temp = 1.0 / f_y;

          for (d_re_tmp = 0; d_re_tmp <= 3 - i; d_re_tmp++) {

            work1[d_re_tmp].re *= temp;

            work1[d_re_tmp].im *= temp;

          }

        }

      }



      if (scale == 0.0) {

        if (-work1[work2_idx_1_re_tmp].im == 0.0) {

          temp = -work1[work2_idx_1_re_tmp].re / work2_idx_3_re;

          scale = 0.0;

        } else if (-work1[work2_idx_1_re_tmp].re == 0.0) {

          temp = 0.0;

          scale = -work1[work2_idx_1_re_tmp].im / work2_idx_3_re;

        } else {

          temp = -work1[work2_idx_1_re_tmp].re / work2_idx_3_re;

          scale = -work1[work2_idx_1_re_tmp].im / work2_idx_3_re;

        }

      } else if (work2_idx_3_re == 0.0) {

        if (-work1[work2_idx_1_re_tmp].re == 0.0) {

          temp = -work1[work2_idx_1_re_tmp].im / scale;

          scale = 0.0;

        } else if (-work1[work2_idx_1_re_tmp].im == 0.0) {

          temp = 0.0;

          scale = -(-work1[work2_idx_1_re_tmp].re / scale);

        } else {

          temp = -work1[work2_idx_1_re_tmp].im / scale;

          scale = -(-work1[work2_idx_1_re_tmp].re / scale);

        }

      } else if (work2_idx_2_im > work2_idx_3_im) {

        work2_idx_2_im = scale / work2_idx_3_re;

        scale = work2_idx_2_im * scale + work2_idx_3_re;

        temp = (work2_idx_2_im * -work1[work2_idx_1_re_tmp].im +

                -work1[work2_idx_1_re_tmp].re) / scale;

        scale = (-work1[work2_idx_1_re_tmp].im - work2_idx_2_im *

                 -work1[work2_idx_1_re_tmp].re) / scale;

      } else if (work2_idx_3_im == work2_idx_2_im) {

        work2_idx_3_re = work2_idx_3_re > 0.0 ? 0.5 : -0.5;

        scale = scale > 0.0 ? 0.5 : -0.5;

        temp = (-work1[work2_idx_1_re_tmp].re * work2_idx_3_re +

                -work1[work2_idx_1_re_tmp].im * scale) / work2_idx_2_im;

        scale = (-work1[work2_idx_1_re_tmp].im * work2_idx_3_re -

                 -work1[work2_idx_1_re_tmp].re * scale) / work2_idx_2_im;

      } else {

        work2_idx_2_im = work2_idx_3_re / scale;

        scale += work2_idx_2_im * work2_idx_3_re;

        temp = (work2_idx_2_im * -work1[work2_idx_1_re_tmp].re +

                -work1[work2_idx_1_re_tmp].im) / scale;

        scale = (work2_idx_2_im * -work1[work2_idx_1_re_tmp].im -

                 (-work1[work2_idx_1_re_tmp].re)) / scale;

      }



      work1[work2_idx_1_re_tmp].re = temp;

      work1[work2_idx_1_re_tmp].im = scale;

      if (work2_idx_1_re_tmp + 1 > 1) {

        if (fabs(work1[work2_idx_1_re_tmp].re) + fabs(work1[work2_idx_1_re_tmp].

             im) > 1.0) {

          temp = 1.0 / (fabs(work1[work2_idx_1_re_tmp].re) + fabs

                        (work1[work2_idx_1_re_tmp].im));

          if (acoeff * rworka[work2_idx_1_re_tmp] >= 1.1235582092889474E+307 *

              temp) {

            for (d_re_tmp = 0; d_re_tmp <= 3 - i; d_re_tmp++) {

              work1[d_re_tmp].re *= temp;

              work1[d_re_tmp].im *= temp;

            }

          }

        }



        work2_idx_3_re = acoeff * work1[work2_idx_1_re_tmp].re;

        scale = acoeff * work1[work2_idx_1_re_tmp].im;

        for (e_jr = 0; e_jr < work2_idx_1_re_tmp; e_jr++) {

          d_re_tmp = d_re_tmp_tmp + e_jr;

          work1[e_jr].re += A[d_re_tmp].re * work2_idx_3_re - A[d_re_tmp].im *

            scale;

          work1[e_jr].im += A[d_re_tmp].im * work2_idx_3_re + A[d_re_tmp].re *

            scale;

        }

      }

    }



    salpha_re = 0.0;

    salpha_im = 0.0;

    acoeff = 0.0;

    dmin = 0.0;

    scale = 0.0;

    work2_idx_2_im = 0.0;

    work2_idx_3_re = 0.0;

    work2_idx_3_im = 0.0;

    for (c_x_tmp = 0; c_x_tmp <= 3 - i; c_x_tmp++) {

      c_j = c_x_tmp << 2;

      salpha_re += V[c_j].re * work1[c_x_tmp].re - V[c_j].im * work1[c_x_tmp].im;

      salpha_im += V[c_j].re * work1[c_x_tmp].im + V[c_j].im * work1[c_x_tmp].re;

      work2_idx_1_re_tmp = c_j + 1;

      acoeff += V[work2_idx_1_re_tmp].re * work1[c_x_tmp].re -

        V[work2_idx_1_re_tmp].im * work1[c_x_tmp].im;

      dmin += V[work2_idx_1_re_tmp].re * work1[c_x_tmp].im +

        V[work2_idx_1_re_tmp].im * work1[c_x_tmp].re;

      work2_idx_1_re_tmp = c_j + 2;

      scale += V[work2_idx_1_re_tmp].re * work1[c_x_tmp].re -

        V[work2_idx_1_re_tmp].im * work1[c_x_tmp].im;

      work2_idx_2_im += V[work2_idx_1_re_tmp].re * work1[c_x_tmp].im +

        V[work2_idx_1_re_tmp].im * work1[c_x_tmp].re;

      c_j += 3;

      work2_idx_3_re += V[c_j].re * work1[c_x_tmp].re - V[c_j].im *

        work1[c_x_tmp].im;

      work2_idx_3_im += V[c_j].re * work1[c_x_tmp].im + V[c_j].im *

        work1[c_x_tmp].re;

    }



    temp = fabs(salpha_re) + fabs(salpha_im);

    f_y = fabs(acoeff) + fabs(dmin);

    if (f_y > temp) {

      temp = f_y;

    }



    f_y = fabs(scale) + fabs(work2_idx_2_im);

    if (f_y > temp) {

      temp = f_y;

    }



    f_y = fabs(work2_idx_3_re) + fabs(work2_idx_3_im);

    if (f_y > temp) {

      temp = f_y;

    }



    if (temp > 2.2250738585072014E-308) {

      temp = 1.0 / temp;

      V[c_x_tmp_tmp].re = temp * salpha_re;

      V[c_x_tmp_tmp].im = temp * salpha_im;

      d_re_tmp = ((3 - i) << 2) + 1;

      V[d_re_tmp].re = temp * acoeff;

      V[d_re_tmp].im = temp * dmin;

      d_re_tmp = ((3 - i) << 2) + 2;

      V[d_re_tmp].re = temp * scale;

      V[d_re_tmp].im = temp * work2_idx_2_im;

      d_re_tmp = ((3 - i) << 2) + 3;

      V[d_re_tmp].re = temp * work2_idx_3_re;

      V[d_re_tmp].im = temp * work2_idx_3_im;

    } else {

      V[c_x_tmp_tmp].re = 0.0;

      V[c_x_tmp_tmp].im = 0.0;

      d_re_tmp = c_x_tmp_tmp + 1;

      V[d_re_tmp].re = 0.0;

      V[d_re_tmp].im = 0.0;

      d_re_tmp = c_x_tmp_tmp + 2;

      V[d_re_tmp].re = 0.0;

      V[d_re_tmp].im = 0.0;

      d_re_tmp = c_x_tmp_tmp + 3;

      V[d_re_tmp].re = 0.0;

      V[d_re_tmp].im = 0.0;

    }

  }

}



// Function for MATLAB Function: '<S9>/MATLAB Function5'

static void cartesian_waypoints_plan_xzggev(creal_T A[16], int32_T *info,

  creal_T alpha1[4], creal_T beta1[4], creal_T V[16])

{

  boolean_T ilascl;

  boolean_T found;

  int32_T exitg1;

  int32_T exitg2;

  boolean_T exitg3;

  boolean_T exitg4;

  *info = 0;

  cartesian_waypoints_planner_B.anrm = cartesian_waypoints_pl_xzlangeM(A);

  if (rtIsInf(cartesian_waypoints_planner_B.anrm) || rtIsNaN

      (cartesian_waypoints_planner_B.anrm)) {

    alpha1[0].re = (rtNaN);

    alpha1[0].im = 0.0;

    beta1[0].re = (rtNaN);

    beta1[0].im = 0.0;

    alpha1[1].re = (rtNaN);

    alpha1[1].im = 0.0;

    beta1[1].re = (rtNaN);

    beta1[1].im = 0.0;

    alpha1[2].re = (rtNaN);

    alpha1[2].im = 0.0;

    beta1[2].re = (rtNaN);

    beta1[2].im = 0.0;

    alpha1[3].re = (rtNaN);

    alpha1[3].im = 0.0;

    beta1[3].re = (rtNaN);

    beta1[3].im = 0.0;

    for (cartesian_waypoints_planner_B.k_m = 0;

         cartesian_waypoints_planner_B.k_m < 16;

         cartesian_waypoints_planner_B.k_m++) {

      V[cartesian_waypoints_planner_B.k_m].re = (rtNaN);

      V[cartesian_waypoints_planner_B.k_m].im = 0.0;

    }

  } else {

    ilascl = false;

    cartesian_waypoints_planner_B.anrmto = cartesian_waypoints_planner_B.anrm;

    if ((cartesian_waypoints_planner_B.anrm > 0.0) &&

        (cartesian_waypoints_planner_B.anrm < 6.7178761075670888E-139)) {

      cartesian_waypoints_planner_B.anrmto = 6.7178761075670888E-139;

      ilascl = true;

      cartesian_waypoints_pla_xzlascl(cartesian_waypoints_planner_B.anrm,

        cartesian_waypoints_planner_B.anrmto, A);

    } else {

      if (cartesian_waypoints_planner_B.anrm > 1.4885657073574029E+138) {

        cartesian_waypoints_planner_B.anrmto = 1.4885657073574029E+138;

        ilascl = true;

        cartesian_waypoints_pla_xzlascl(cartesian_waypoints_planner_B.anrm,

          cartesian_waypoints_planner_B.anrmto, A);

      }

    }



    cartesian_waypoints_planner_B.rscale[0] = 1;

    cartesian_waypoints_planner_B.rscale[1] = 1;

    cartesian_waypoints_planner_B.rscale[2] = 1;

    cartesian_waypoints_planner_B.rscale[3] = 1;

    cartesian_waypoints_planner_B.c_i = 0;

    cartesian_waypoints_planner_B.ihi = 4;

    do {

      exitg2 = 0;

      cartesian_waypoints_planner_B.i_p5 = 0;

      cartesian_waypoints_planner_B.jcol = 0;

      found = false;

      cartesian_waypoints_planner_B.ii = cartesian_waypoints_planner_B.ihi;

      exitg3 = false;

      while ((!exitg3) && (cartesian_waypoints_planner_B.ii > 0)) {

        cartesian_waypoints_planner_B.nzcount = 0;

        cartesian_waypoints_planner_B.i_p5 = cartesian_waypoints_planner_B.ii;

        cartesian_waypoints_planner_B.jcol = cartesian_waypoints_planner_B.ihi;

        cartesian_waypoints_planner_B.jj = 0;

        exitg4 = false;

        while ((!exitg4) && (cartesian_waypoints_planner_B.jj <=

                             cartesian_waypoints_planner_B.ihi - 1)) {

          cartesian_waypoints_planner_B.k_m = ((cartesian_waypoints_planner_B.jj

            << 2) + cartesian_waypoints_planner_B.ii) - 1;

          if ((A[cartesian_waypoints_planner_B.k_m].re != 0.0) ||

              (A[cartesian_waypoints_planner_B.k_m].im != 0.0) ||

              (cartesian_waypoints_planner_B.jj + 1 ==

               cartesian_waypoints_planner_B.ii)) {

            if (cartesian_waypoints_planner_B.nzcount == 0) {

              cartesian_waypoints_planner_B.jcol =

                cartesian_waypoints_planner_B.jj + 1;

              cartesian_waypoints_planner_B.nzcount = 1;

              cartesian_waypoints_planner_B.jj++;

            } else {

              cartesian_waypoints_planner_B.nzcount = 2;

              exitg4 = true;

            }

          } else {

            cartesian_waypoints_planner_B.jj++;

          }

        }



        if (cartesian_waypoints_planner_B.nzcount < 2) {

          found = true;

          exitg3 = true;

        } else {

          cartesian_waypoints_planner_B.ii--;

        }

      }



      if (!found) {

        exitg2 = 2;

      } else {

        if (cartesian_waypoints_planner_B.i_p5 !=

            cartesian_waypoints_planner_B.ihi) {

          cartesian_waypoints_planner_B.atmp_re =

            A[cartesian_waypoints_planner_B.i_p5 - 1].re;

          cartesian_waypoints_planner_B.atmp_im =

            A[cartesian_waypoints_planner_B.i_p5 - 1].im;

          A[cartesian_waypoints_planner_B.i_p5 - 1] =

            A[cartesian_waypoints_planner_B.ihi - 1];

          A[cartesian_waypoints_planner_B.ihi - 1].re =

            cartesian_waypoints_planner_B.atmp_re;

          A[cartesian_waypoints_planner_B.ihi - 1].im =

            cartesian_waypoints_planner_B.atmp_im;

          cartesian_waypoints_planner_B.atmp_re =

            A[cartesian_waypoints_planner_B.i_p5 + 3].re;

          cartesian_waypoints_planner_B.atmp_im =

            A[cartesian_waypoints_planner_B.i_p5 + 3].im;

          A[cartesian_waypoints_planner_B.i_p5 + 3] =

            A[cartesian_waypoints_planner_B.ihi + 3];

          A[cartesian_waypoints_planner_B.ihi + 3].re =

            cartesian_waypoints_planner_B.atmp_re;

          A[cartesian_waypoints_planner_B.ihi + 3].im =

            cartesian_waypoints_planner_B.atmp_im;

          cartesian_waypoints_planner_B.atmp_re =

            A[cartesian_waypoints_planner_B.i_p5 + 7].re;

          cartesian_waypoints_planner_B.atmp_im =

            A[cartesian_waypoints_planner_B.i_p5 + 7].im;

          A[cartesian_waypoints_planner_B.i_p5 + 7] =

            A[cartesian_waypoints_planner_B.ihi + 7];

          A[cartesian_waypoints_planner_B.ihi + 7].re =

            cartesian_waypoints_planner_B.atmp_re;

          A[cartesian_waypoints_planner_B.ihi + 7].im =

            cartesian_waypoints_planner_B.atmp_im;

          cartesian_waypoints_planner_B.atmp_re =

            A[cartesian_waypoints_planner_B.i_p5 + 11].re;

          cartesian_waypoints_planner_B.atmp_im =

            A[cartesian_waypoints_planner_B.i_p5 + 11].im;

          A[cartesian_waypoints_planner_B.i_p5 + 11] =

            A[cartesian_waypoints_planner_B.ihi + 11];

          A[cartesian_waypoints_planner_B.ihi + 11].re =

            cartesian_waypoints_planner_B.atmp_re;

          A[cartesian_waypoints_planner_B.ihi + 11].im =

            cartesian_waypoints_planner_B.atmp_im;

        }



        if (cartesian_waypoints_planner_B.jcol !=

            cartesian_waypoints_planner_B.ihi) {

          cartesian_waypoints_planner_B.ii = 0;

          while (cartesian_waypoints_planner_B.ii <=

                 cartesian_waypoints_planner_B.ihi - 1) {

            cartesian_waypoints_planner_B.i_p5 =

              ((cartesian_waypoints_planner_B.jcol - 1) << 2) +

              cartesian_waypoints_planner_B.ii;

            cartesian_waypoints_planner_B.atmp_re =

              A[cartesian_waypoints_planner_B.i_p5].re;

            cartesian_waypoints_planner_B.atmp_im =

              A[cartesian_waypoints_planner_B.i_p5].im;

            cartesian_waypoints_planner_B.k_m =

              ((cartesian_waypoints_planner_B.ihi - 1) << 2) +

              cartesian_waypoints_planner_B.ii;

            A[cartesian_waypoints_planner_B.i_p5] =

              A[cartesian_waypoints_planner_B.k_m];

            A[cartesian_waypoints_planner_B.k_m].re =

              cartesian_waypoints_planner_B.atmp_re;

            A[cartesian_waypoints_planner_B.k_m].im =

              cartesian_waypoints_planner_B.atmp_im;

            cartesian_waypoints_planner_B.ii++;

          }

        }



        cartesian_waypoints_planner_B.rscale[cartesian_waypoints_planner_B.ihi -

          1] = cartesian_waypoints_planner_B.jcol;

        cartesian_waypoints_planner_B.ihi--;

        if (cartesian_waypoints_planner_B.ihi == 1) {

          cartesian_waypoints_planner_B.rscale[0] = 1;

          exitg2 = 1;

        }

      }

    } while (exitg2 == 0);



    if (exitg2 == 1) {

    } else {

      do {

        exitg1 = 0;

        cartesian_waypoints_planner_B.ii = 0;

        cartesian_waypoints_planner_B.jcol = 0;

        found = false;

        cartesian_waypoints_planner_B.i_p5 = cartesian_waypoints_planner_B.c_i +

          1;

        exitg3 = false;

        while ((!exitg3) && (cartesian_waypoints_planner_B.i_p5 <=

                             cartesian_waypoints_planner_B.ihi)) {

          cartesian_waypoints_planner_B.nzcount = 0;

          cartesian_waypoints_planner_B.ii = cartesian_waypoints_planner_B.ihi;

          cartesian_waypoints_planner_B.jcol =

            cartesian_waypoints_planner_B.i_p5;

          cartesian_waypoints_planner_B.jj = cartesian_waypoints_planner_B.c_i +

            1;

          exitg4 = false;

          while ((!exitg4) && (cartesian_waypoints_planner_B.jj <=

                               cartesian_waypoints_planner_B.ihi)) {

            cartesian_waypoints_planner_B.k_m =

              (((cartesian_waypoints_planner_B.i_p5 - 1) << 2) +

               cartesian_waypoints_planner_B.jj) - 1;

            if ((A[cartesian_waypoints_planner_B.k_m].re != 0.0) ||

                (A[cartesian_waypoints_planner_B.k_m].im != 0.0) ||

                (cartesian_waypoints_planner_B.jj ==

                 cartesian_waypoints_planner_B.i_p5)) {

              if (cartesian_waypoints_planner_B.nzcount == 0) {

                cartesian_waypoints_planner_B.ii =

                  cartesian_waypoints_planner_B.jj;

                cartesian_waypoints_planner_B.nzcount = 1;

                cartesian_waypoints_planner_B.jj++;

              } else {

                cartesian_waypoints_planner_B.nzcount = 2;

                exitg4 = true;

              }

            } else {

              cartesian_waypoints_planner_B.jj++;

            }

          }



          if (cartesian_waypoints_planner_B.nzcount < 2) {

            found = true;

            exitg3 = true;

          } else {

            cartesian_waypoints_planner_B.i_p5++;

          }

        }



        if (!found) {

          exitg1 = 1;

        } else {

          if (cartesian_waypoints_planner_B.c_i + 1 !=

              cartesian_waypoints_planner_B.ii) {

            cartesian_waypoints_planner_B.nzcount =

              cartesian_waypoints_planner_B.c_i;

            while (cartesian_waypoints_planner_B.nzcount + 1 < 5) {

              cartesian_waypoints_planner_B.k_m =

                cartesian_waypoints_planner_B.nzcount << 2;

              cartesian_waypoints_planner_B.i_p5 =

                (cartesian_waypoints_planner_B.k_m +

                 cartesian_waypoints_planner_B.ii) - 1;

              cartesian_waypoints_planner_B.atmp_re =

                A[cartesian_waypoints_planner_B.i_p5].re;

              cartesian_waypoints_planner_B.atmp_im =

                A[cartesian_waypoints_planner_B.i_p5].im;

              cartesian_waypoints_planner_B.k_m +=

                cartesian_waypoints_planner_B.c_i;

              A[cartesian_waypoints_planner_B.i_p5] =

                A[cartesian_waypoints_planner_B.k_m];

              A[cartesian_waypoints_planner_B.k_m].re =

                cartesian_waypoints_planner_B.atmp_re;

              A[cartesian_waypoints_planner_B.k_m].im =

                cartesian_waypoints_planner_B.atmp_im;

              cartesian_waypoints_planner_B.nzcount++;

            }

          }



          if (cartesian_waypoints_planner_B.c_i + 1 !=

              cartesian_waypoints_planner_B.jcol) {

            cartesian_waypoints_planner_B.ii = 0;

            while (cartesian_waypoints_planner_B.ii <=

                   cartesian_waypoints_planner_B.ihi - 1) {

              cartesian_waypoints_planner_B.i_p5 =

                ((cartesian_waypoints_planner_B.jcol - 1) << 2) +

                cartesian_waypoints_planner_B.ii;

              cartesian_waypoints_planner_B.atmp_re =

                A[cartesian_waypoints_planner_B.i_p5].re;

              cartesian_waypoints_planner_B.atmp_im =

                A[cartesian_waypoints_planner_B.i_p5].im;

              cartesian_waypoints_planner_B.k_m =

                (cartesian_waypoints_planner_B.c_i << 2) +

                cartesian_waypoints_planner_B.ii;

              A[cartesian_waypoints_planner_B.i_p5] =

                A[cartesian_waypoints_planner_B.k_m];

              A[cartesian_waypoints_planner_B.k_m].re =

                cartesian_waypoints_planner_B.atmp_re;

              A[cartesian_waypoints_planner_B.k_m].im =

                cartesian_waypoints_planner_B.atmp_im;

              cartesian_waypoints_planner_B.ii++;

            }

          }



          cartesian_waypoints_planner_B.rscale[cartesian_waypoints_planner_B.c_i]

            = cartesian_waypoints_planner_B.jcol;

          cartesian_waypoints_planner_B.c_i++;

          if (cartesian_waypoints_planner_B.c_i + 1 ==

              cartesian_waypoints_planner_B.ihi) {

            cartesian_waypoints_planner_B.rscale[cartesian_waypoints_planner_B.c_i]

              = cartesian_waypoints_planner_B.c_i + 1;

            exitg1 = 1;

          }

        }

      } while (exitg1 == 0);

    }



    for (cartesian_waypoints_planner_B.k_m = 0;

         cartesian_waypoints_planner_B.k_m < 16;

         cartesian_waypoints_planner_B.k_m++) {

      cartesian_waypoints_planner_B.b_I_n[cartesian_waypoints_planner_B.k_m] = 0;

    }



    cartesian_waypoints_planner_B.b_I_n[0] = 1;

    cartesian_waypoints_planner_B.b_I_n[5] = 1;

    cartesian_waypoints_planner_B.b_I_n[10] = 1;

    cartesian_waypoints_planner_B.b_I_n[15] = 1;

    for (cartesian_waypoints_planner_B.k_m = 0;

         cartesian_waypoints_planner_B.k_m < 16;

         cartesian_waypoints_planner_B.k_m++) {

      V[cartesian_waypoints_planner_B.k_m].re =

        cartesian_waypoints_planner_B.b_I_n[cartesian_waypoints_planner_B.k_m];

      V[cartesian_waypoints_planner_B.k_m].im = 0.0;

    }



    if (cartesian_waypoints_planner_B.ihi >= cartesian_waypoints_planner_B.c_i +

        3) {

      cartesian_waypoints_planner_B.jcol = cartesian_waypoints_planner_B.c_i;

      while (cartesian_waypoints_planner_B.jcol + 1 <

             cartesian_waypoints_planner_B.ihi - 1) {

        cartesian_waypoints_planner_B.ii = cartesian_waypoints_planner_B.ihi - 1;

        while (cartesian_waypoints_planner_B.ii + 1 >

               cartesian_waypoints_planner_B.jcol + 2) {

          cartesian_waypoints_pla_xzlartg(A[(cartesian_waypoints_planner_B.ii +

            (cartesian_waypoints_planner_B.jcol << 2)) - 1],

            A[cartesian_waypoints_planner_B.ii +

            (cartesian_waypoints_planner_B.jcol << 2)],

            &cartesian_waypoints_planner_B.mul,

            &cartesian_waypoints_planner_B.s_c, &A

            [(cartesian_waypoints_planner_B.ii +

              (cartesian_waypoints_planner_B.jcol << 2)) - 1]);

          cartesian_waypoints_planner_B.k_m = cartesian_waypoints_planner_B.ii +

            (cartesian_waypoints_planner_B.jcol << 2);

          A[cartesian_waypoints_planner_B.k_m].re = 0.0;

          A[cartesian_waypoints_planner_B.k_m].im = 0.0;

          cartesian_waypoints_planner_B.nzcount =

            cartesian_waypoints_planner_B.jcol + 1;

          while (cartesian_waypoints_planner_B.nzcount + 1 < 5) {

            cartesian_waypoints_planner_B.i_p5 =

              (cartesian_waypoints_planner_B.nzcount << 2) +

              cartesian_waypoints_planner_B.ii;

            cartesian_waypoints_planner_B.k_m =

              cartesian_waypoints_planner_B.i_p5 - 1;

            cartesian_waypoints_planner_B.atmp_re =

              A[cartesian_waypoints_planner_B.k_m].re *

              cartesian_waypoints_planner_B.mul +

              (A[cartesian_waypoints_planner_B.i_p5].re *

               cartesian_waypoints_planner_B.s_c.re -

               A[cartesian_waypoints_planner_B.i_p5].im *

               cartesian_waypoints_planner_B.s_c.im);

            cartesian_waypoints_planner_B.atmp_im =

              A[cartesian_waypoints_planner_B.k_m].im *

              cartesian_waypoints_planner_B.mul +

              (A[cartesian_waypoints_planner_B.i_p5].im *

               cartesian_waypoints_planner_B.s_c.re +

               A[cartesian_waypoints_planner_B.i_p5].re *

               cartesian_waypoints_planner_B.s_c.im);

            cartesian_waypoints_planner_B.d =

              A[cartesian_waypoints_planner_B.k_m].im;

            cartesian_waypoints_planner_B.d1 =

              A[cartesian_waypoints_planner_B.k_m].re;

            A[cartesian_waypoints_planner_B.i_p5].re =

              A[cartesian_waypoints_planner_B.i_p5].re *

              cartesian_waypoints_planner_B.mul -

              (A[cartesian_waypoints_planner_B.k_m].re *

               cartesian_waypoints_planner_B.s_c.re +

               A[cartesian_waypoints_planner_B.k_m].im *

               cartesian_waypoints_planner_B.s_c.im);

            A[cartesian_waypoints_planner_B.i_p5].im =

              A[cartesian_waypoints_planner_B.i_p5].im *

              cartesian_waypoints_planner_B.mul -

              (cartesian_waypoints_planner_B.s_c.re *

               cartesian_waypoints_planner_B.d -

               cartesian_waypoints_planner_B.s_c.im *

               cartesian_waypoints_planner_B.d1);

            A[cartesian_waypoints_planner_B.k_m].re =

              cartesian_waypoints_planner_B.atmp_re;

            A[cartesian_waypoints_planner_B.k_m].im =

              cartesian_waypoints_planner_B.atmp_im;

            cartesian_waypoints_planner_B.nzcount++;

          }



          cartesian_waypoints_planner_B.s_c.re =

            -cartesian_waypoints_planner_B.s_c.re;

          cartesian_waypoints_planner_B.s_c.im =

            -cartesian_waypoints_planner_B.s_c.im;

          cartesian_waypoints_planner_B.nzcount = 0;

          while (cartesian_waypoints_planner_B.nzcount + 1 <=

                 cartesian_waypoints_planner_B.ihi) {

            cartesian_waypoints_planner_B.i_p5 =

              ((cartesian_waypoints_planner_B.ii - 1) << 2) +

              cartesian_waypoints_planner_B.nzcount;

            cartesian_waypoints_planner_B.k_m =

              (cartesian_waypoints_planner_B.ii << 2) +

              cartesian_waypoints_planner_B.nzcount;

            cartesian_waypoints_planner_B.atmp_re =

              (A[cartesian_waypoints_planner_B.i_p5].re *

               cartesian_waypoints_planner_B.s_c.re -

               A[cartesian_waypoints_planner_B.i_p5].im *

               cartesian_waypoints_planner_B.s_c.im) +

              A[cartesian_waypoints_planner_B.k_m].re *

              cartesian_waypoints_planner_B.mul;

            cartesian_waypoints_planner_B.atmp_im =

              (A[cartesian_waypoints_planner_B.i_p5].im *

               cartesian_waypoints_planner_B.s_c.re +

               A[cartesian_waypoints_planner_B.i_p5].re *

               cartesian_waypoints_planner_B.s_c.im) +

              A[cartesian_waypoints_planner_B.k_m].im *

              cartesian_waypoints_planner_B.mul;

            cartesian_waypoints_planner_B.d =

              A[cartesian_waypoints_planner_B.k_m].im;

            cartesian_waypoints_planner_B.d1 =

              A[cartesian_waypoints_planner_B.k_m].re;

            A[cartesian_waypoints_planner_B.i_p5].re =

              A[cartesian_waypoints_planner_B.i_p5].re *

              cartesian_waypoints_planner_B.mul -

              (A[cartesian_waypoints_planner_B.k_m].re *

               cartesian_waypoints_planner_B.s_c.re +

               A[cartesian_waypoints_planner_B.k_m].im *

               cartesian_waypoints_planner_B.s_c.im);

            A[cartesian_waypoints_planner_B.i_p5].im =

              A[cartesian_waypoints_planner_B.i_p5].im *

              cartesian_waypoints_planner_B.mul -

              (cartesian_waypoints_planner_B.s_c.re *

               cartesian_waypoints_planner_B.d -

               cartesian_waypoints_planner_B.s_c.im *

               cartesian_waypoints_planner_B.d1);

            A[cartesian_waypoints_planner_B.k_m].re =

              cartesian_waypoints_planner_B.atmp_re;

            A[cartesian_waypoints_planner_B.k_m].im =

              cartesian_waypoints_planner_B.atmp_im;

            cartesian_waypoints_planner_B.nzcount++;

          }



          cartesian_waypoints_planner_B.i_p5 = (cartesian_waypoints_planner_B.ii

            - 1) << 2;

          cartesian_waypoints_planner_B.k_m = cartesian_waypoints_planner_B.ii <<

            2;

          cartesian_waypoints_planner_B.atmp_re =

            (V[cartesian_waypoints_planner_B.i_p5].re *

             cartesian_waypoints_planner_B.s_c.re -

             V[cartesian_waypoints_planner_B.i_p5].im *

             cartesian_waypoints_planner_B.s_c.im) +

            V[cartesian_waypoints_planner_B.k_m].re *

            cartesian_waypoints_planner_B.mul;

          cartesian_waypoints_planner_B.atmp_im =

            (V[cartesian_waypoints_planner_B.i_p5].im *

             cartesian_waypoints_planner_B.s_c.re +

             V[cartesian_waypoints_planner_B.i_p5].re *

             cartesian_waypoints_planner_B.s_c.im) +

            V[cartesian_waypoints_planner_B.k_m].im *

            cartesian_waypoints_planner_B.mul;

          cartesian_waypoints_planner_B.d = V[cartesian_waypoints_planner_B.k_m]

            .re;

          V[cartesian_waypoints_planner_B.i_p5].re =

            V[cartesian_waypoints_planner_B.i_p5].re *

            cartesian_waypoints_planner_B.mul -

            (V[cartesian_waypoints_planner_B.k_m].re *

             cartesian_waypoints_planner_B.s_c.re +

             V[cartesian_waypoints_planner_B.k_m].im *

             cartesian_waypoints_planner_B.s_c.im);

          V[cartesian_waypoints_planner_B.i_p5].im =

            V[cartesian_waypoints_planner_B.i_p5].im *

            cartesian_waypoints_planner_B.mul -

            (V[cartesian_waypoints_planner_B.k_m].im *

             cartesian_waypoints_planner_B.s_c.re -

             cartesian_waypoints_planner_B.s_c.im *

             cartesian_waypoints_planner_B.d);

          V[cartesian_waypoints_planner_B.k_m].re =

            cartesian_waypoints_planner_B.atmp_re;

          V[cartesian_waypoints_planner_B.k_m].im =

            cartesian_waypoints_planner_B.atmp_im;

          cartesian_waypoints_planner_B.nzcount =

            cartesian_waypoints_planner_B.i_p5 + 1;

          cartesian_waypoints_planner_B.jj = cartesian_waypoints_planner_B.k_m +

            1;

          cartesian_waypoints_planner_B.atmp_re =

            (V[cartesian_waypoints_planner_B.nzcount].re *

             cartesian_waypoints_planner_B.s_c.re -

             V[cartesian_waypoints_planner_B.nzcount].im *

             cartesian_waypoints_planner_B.s_c.im) +

            V[cartesian_waypoints_planner_B.jj].re *

            cartesian_waypoints_planner_B.mul;

          cartesian_waypoints_planner_B.atmp_im =

            (V[cartesian_waypoints_planner_B.nzcount].im *

             cartesian_waypoints_planner_B.s_c.re +

             V[cartesian_waypoints_planner_B.nzcount].re *

             cartesian_waypoints_planner_B.s_c.im) +

            V[cartesian_waypoints_planner_B.jj].im *

            cartesian_waypoints_planner_B.mul;

          cartesian_waypoints_planner_B.d = V[cartesian_waypoints_planner_B.jj].

            re;

          V[cartesian_waypoints_planner_B.nzcount].re =

            V[cartesian_waypoints_planner_B.nzcount].re *

            cartesian_waypoints_planner_B.mul -

            (V[cartesian_waypoints_planner_B.jj].re *

             cartesian_waypoints_planner_B.s_c.re +

             V[cartesian_waypoints_planner_B.jj].im *

             cartesian_waypoints_planner_B.s_c.im);

          V[cartesian_waypoints_planner_B.nzcount].im =

            V[cartesian_waypoints_planner_B.nzcount].im *

            cartesian_waypoints_planner_B.mul -

            (V[cartesian_waypoints_planner_B.jj].im *

             cartesian_waypoints_planner_B.s_c.re -

             cartesian_waypoints_planner_B.s_c.im *

             cartesian_waypoints_planner_B.d);

          V[cartesian_waypoints_planner_B.jj].re =

            cartesian_waypoints_planner_B.atmp_re;

          V[cartesian_waypoints_planner_B.jj].im =

            cartesian_waypoints_planner_B.atmp_im;

          cartesian_waypoints_planner_B.nzcount =

            cartesian_waypoints_planner_B.i_p5 + 2;

          cartesian_waypoints_planner_B.jj = cartesian_waypoints_planner_B.k_m +

            2;

          cartesian_waypoints_planner_B.atmp_re =

            (V[cartesian_waypoints_planner_B.nzcount].re *

             cartesian_waypoints_planner_B.s_c.re -

             V[cartesian_waypoints_planner_B.nzcount].im *

             cartesian_waypoints_planner_B.s_c.im) +

            V[cartesian_waypoints_planner_B.jj].re *

            cartesian_waypoints_planner_B.mul;

          cartesian_waypoints_planner_B.atmp_im =

            (V[cartesian_waypoints_planner_B.nzcount].im *

             cartesian_waypoints_planner_B.s_c.re +

             V[cartesian_waypoints_planner_B.nzcount].re *

             cartesian_waypoints_planner_B.s_c.im) +

            V[cartesian_waypoints_planner_B.jj].im *

            cartesian_waypoints_planner_B.mul;

          cartesian_waypoints_planner_B.d = V[cartesian_waypoints_planner_B.jj].

            re;

          V[cartesian_waypoints_planner_B.nzcount].re =

            V[cartesian_waypoints_planner_B.nzcount].re *

            cartesian_waypoints_planner_B.mul -

            (V[cartesian_waypoints_planner_B.jj].re *

             cartesian_waypoints_planner_B.s_c.re +

             V[cartesian_waypoints_planner_B.jj].im *

             cartesian_waypoints_planner_B.s_c.im);

          V[cartesian_waypoints_planner_B.nzcount].im =

            V[cartesian_waypoints_planner_B.nzcount].im *

            cartesian_waypoints_planner_B.mul -

            (V[cartesian_waypoints_planner_B.jj].im *

             cartesian_waypoints_planner_B.s_c.re -

             cartesian_waypoints_planner_B.s_c.im *

             cartesian_waypoints_planner_B.d);

          V[cartesian_waypoints_planner_B.jj].re =

            cartesian_waypoints_planner_B.atmp_re;

          V[cartesian_waypoints_planner_B.jj].im =

            cartesian_waypoints_planner_B.atmp_im;

          cartesian_waypoints_planner_B.i_p5 += 3;

          cartesian_waypoints_planner_B.k_m += 3;

          cartesian_waypoints_planner_B.atmp_re =

            (V[cartesian_waypoints_planner_B.i_p5].re *

             cartesian_waypoints_planner_B.s_c.re -

             V[cartesian_waypoints_planner_B.i_p5].im *

             cartesian_waypoints_planner_B.s_c.im) +

            V[cartesian_waypoints_planner_B.k_m].re *

            cartesian_waypoints_planner_B.mul;

          cartesian_waypoints_planner_B.atmp_im =

            (V[cartesian_waypoints_planner_B.i_p5].im *

             cartesian_waypoints_planner_B.s_c.re +

             V[cartesian_waypoints_planner_B.i_p5].re *

             cartesian_waypoints_planner_B.s_c.im) +

            V[cartesian_waypoints_planner_B.k_m].im *

            cartesian_waypoints_planner_B.mul;

          cartesian_waypoints_planner_B.d = V[cartesian_waypoints_planner_B.k_m]

            .re;

          V[cartesian_waypoints_planner_B.i_p5].re =

            V[cartesian_waypoints_planner_B.i_p5].re *

            cartesian_waypoints_planner_B.mul -

            (V[cartesian_waypoints_planner_B.k_m].re *

             cartesian_waypoints_planner_B.s_c.re +

             V[cartesian_waypoints_planner_B.k_m].im *

             cartesian_waypoints_planner_B.s_c.im);

          V[cartesian_waypoints_planner_B.i_p5].im =

            V[cartesian_waypoints_planner_B.i_p5].im *

            cartesian_waypoints_planner_B.mul -

            (V[cartesian_waypoints_planner_B.k_m].im *

             cartesian_waypoints_planner_B.s_c.re -

             cartesian_waypoints_planner_B.s_c.im *

             cartesian_waypoints_planner_B.d);

          V[cartesian_waypoints_planner_B.k_m].re =

            cartesian_waypoints_planner_B.atmp_re;

          V[cartesian_waypoints_planner_B.k_m].im =

            cartesian_waypoints_planner_B.atmp_im;

          cartesian_waypoints_planner_B.ii--;

        }



        cartesian_waypoints_planner_B.jcol++;

      }

    }



    cartesian_waypoints_pla_xzhgeqz(A, cartesian_waypoints_planner_B.c_i + 1,

      cartesian_waypoints_planner_B.ihi, V, &cartesian_waypoints_planner_B.k_m,

      alpha1, beta1);

    *info = cartesian_waypoints_planner_B.k_m;

    if (*info == 0) {

      cartesian_waypoints_pla_xztgevc(A, V);

      if (cartesian_waypoints_planner_B.c_i + 1 > 1) {

        cartesian_waypoints_planner_B.c_i--;

        while (cartesian_waypoints_planner_B.c_i + 1 >= 1) {

          cartesian_waypoints_planner_B.k_m =

            cartesian_waypoints_planner_B.rscale[cartesian_waypoints_planner_B.c_i]

            - 1;

          if (cartesian_waypoints_planner_B.c_i + 1 !=

              cartesian_waypoints_planner_B.rscale[cartesian_waypoints_planner_B.c_i])

          {

            cartesian_waypoints_planner_B.atmp_re =

              V[cartesian_waypoints_planner_B.c_i].re;

            cartesian_waypoints_planner_B.atmp_im =

              V[cartesian_waypoints_planner_B.c_i].im;

            V[cartesian_waypoints_planner_B.c_i] =

              V[cartesian_waypoints_planner_B.k_m];

            V[cartesian_waypoints_planner_B.k_m].re =

              cartesian_waypoints_planner_B.atmp_re;

            V[cartesian_waypoints_planner_B.k_m].im =

              cartesian_waypoints_planner_B.atmp_im;

            cartesian_waypoints_planner_B.atmp_re =

              V[cartesian_waypoints_planner_B.c_i + 4].re;

            cartesian_waypoints_planner_B.atmp_im =

              V[cartesian_waypoints_planner_B.c_i + 4].im;

            V[cartesian_waypoints_planner_B.c_i + 4] =

              V[cartesian_waypoints_planner_B.k_m + 4];

            V[cartesian_waypoints_planner_B.k_m + 4].re =

              cartesian_waypoints_planner_B.atmp_re;

            V[cartesian_waypoints_planner_B.k_m + 4].im =

              cartesian_waypoints_planner_B.atmp_im;

            cartesian_waypoints_planner_B.atmp_re =

              V[cartesian_waypoints_planner_B.c_i + 8].re;

            cartesian_waypoints_planner_B.atmp_im =

              V[cartesian_waypoints_planner_B.c_i + 8].im;

            V[cartesian_waypoints_planner_B.c_i + 8] =

              V[cartesian_waypoints_planner_B.k_m + 8];

            V[cartesian_waypoints_planner_B.k_m + 8].re =

              cartesian_waypoints_planner_B.atmp_re;

            V[cartesian_waypoints_planner_B.k_m + 8].im =

              cartesian_waypoints_planner_B.atmp_im;

            cartesian_waypoints_planner_B.atmp_re =

              V[cartesian_waypoints_planner_B.c_i + 12].re;

            cartesian_waypoints_planner_B.atmp_im =

              V[cartesian_waypoints_planner_B.c_i + 12].im;

            V[cartesian_waypoints_planner_B.c_i + 12] =

              V[cartesian_waypoints_planner_B.k_m + 12];

            V[cartesian_waypoints_planner_B.k_m + 12].re =

              cartesian_waypoints_planner_B.atmp_re;

            V[cartesian_waypoints_planner_B.k_m + 12].im =

              cartesian_waypoints_planner_B.atmp_im;

          }



          cartesian_waypoints_planner_B.c_i--;

        }

      }



      if (cartesian_waypoints_planner_B.ihi < 4) {

        while (cartesian_waypoints_planner_B.ihi + 1 < 5) {

          cartesian_waypoints_planner_B.k_m =

            cartesian_waypoints_planner_B.rscale[cartesian_waypoints_planner_B.ihi]

            - 1;

          if (cartesian_waypoints_planner_B.ihi + 1 !=

              cartesian_waypoints_planner_B.rscale[cartesian_waypoints_planner_B.ihi])

          {

            cartesian_waypoints_planner_B.atmp_re =

              V[cartesian_waypoints_planner_B.ihi].re;

            cartesian_waypoints_planner_B.atmp_im =

              V[cartesian_waypoints_planner_B.ihi].im;

            V[cartesian_waypoints_planner_B.ihi] =

              V[cartesian_waypoints_planner_B.k_m];

            V[cartesian_waypoints_planner_B.k_m].re =

              cartesian_waypoints_planner_B.atmp_re;

            V[cartesian_waypoints_planner_B.k_m].im =

              cartesian_waypoints_planner_B.atmp_im;

            cartesian_waypoints_planner_B.atmp_re =

              V[cartesian_waypoints_planner_B.ihi + 4].re;

            cartesian_waypoints_planner_B.atmp_im =

              V[cartesian_waypoints_planner_B.ihi + 4].im;

            V[cartesian_waypoints_planner_B.ihi + 4] =

              V[cartesian_waypoints_planner_B.k_m + 4];

            V[cartesian_waypoints_planner_B.k_m + 4].re =

              cartesian_waypoints_planner_B.atmp_re;

            V[cartesian_waypoints_planner_B.k_m + 4].im =

              cartesian_waypoints_planner_B.atmp_im;

            cartesian_waypoints_planner_B.atmp_re =

              V[cartesian_waypoints_planner_B.ihi + 8].re;

            cartesian_waypoints_planner_B.atmp_im =

              V[cartesian_waypoints_planner_B.ihi + 8].im;

            V[cartesian_waypoints_planner_B.ihi + 8] =

              V[cartesian_waypoints_planner_B.k_m + 8];

            V[cartesian_waypoints_planner_B.k_m + 8].re =

              cartesian_waypoints_planner_B.atmp_re;

            V[cartesian_waypoints_planner_B.k_m + 8].im =

              cartesian_waypoints_planner_B.atmp_im;

            cartesian_waypoints_planner_B.atmp_re =

              V[cartesian_waypoints_planner_B.ihi + 12].re;

            cartesian_waypoints_planner_B.atmp_im =

              V[cartesian_waypoints_planner_B.ihi + 12].im;

            V[cartesian_waypoints_planner_B.ihi + 12] =

              V[cartesian_waypoints_planner_B.k_m + 12];

            V[cartesian_waypoints_planner_B.k_m + 12].re =

              cartesian_waypoints_planner_B.atmp_re;

            V[cartesian_waypoints_planner_B.k_m + 12].im =

              cartesian_waypoints_planner_B.atmp_im;

          }



          cartesian_waypoints_planner_B.ihi++;

        }

      }



      for (cartesian_waypoints_planner_B.ihi = 0;

           cartesian_waypoints_planner_B.ihi < 4;

           cartesian_waypoints_planner_B.ihi++) {

        cartesian_waypoints_planner_B.c_i = cartesian_waypoints_planner_B.ihi <<

          2;

        cartesian_waypoints_planner_B.atmp_re = fabs

          (V[cartesian_waypoints_planner_B.c_i].re) + fabs

          (V[cartesian_waypoints_planner_B.c_i].im);

        cartesian_waypoints_planner_B.k_m = cartesian_waypoints_planner_B.c_i +

          1;

        cartesian_waypoints_planner_B.atmp_im = fabs

          (V[cartesian_waypoints_planner_B.k_m].re) + fabs

          (V[cartesian_waypoints_planner_B.k_m].im);

        if (cartesian_waypoints_planner_B.atmp_im >

            cartesian_waypoints_planner_B.atmp_re) {

          cartesian_waypoints_planner_B.atmp_re =

            cartesian_waypoints_planner_B.atmp_im;

        }



        cartesian_waypoints_planner_B.i_p5 = cartesian_waypoints_planner_B.c_i +

          2;

        cartesian_waypoints_planner_B.atmp_im = fabs

          (V[cartesian_waypoints_planner_B.i_p5].re) + fabs

          (V[cartesian_waypoints_planner_B.i_p5].im);

        if (cartesian_waypoints_planner_B.atmp_im >

            cartesian_waypoints_planner_B.atmp_re) {

          cartesian_waypoints_planner_B.atmp_re =

            cartesian_waypoints_planner_B.atmp_im;

        }



        cartesian_waypoints_planner_B.jcol = cartesian_waypoints_planner_B.c_i +

          3;

        cartesian_waypoints_planner_B.atmp_im = fabs

          (V[cartesian_waypoints_planner_B.jcol].re) + fabs

          (V[cartesian_waypoints_planner_B.jcol].im);

        if (cartesian_waypoints_planner_B.atmp_im >

            cartesian_waypoints_planner_B.atmp_re) {

          cartesian_waypoints_planner_B.atmp_re =

            cartesian_waypoints_planner_B.atmp_im;

        }



        if (cartesian_waypoints_planner_B.atmp_re >= 6.7178761075670888E-139) {

          cartesian_waypoints_planner_B.atmp_re = 1.0 /

            cartesian_waypoints_planner_B.atmp_re;

          V[cartesian_waypoints_planner_B.c_i].re *=

            cartesian_waypoints_planner_B.atmp_re;

          V[cartesian_waypoints_planner_B.c_i].im *=

            cartesian_waypoints_planner_B.atmp_re;

          V[cartesian_waypoints_planner_B.k_m].re *=

            cartesian_waypoints_planner_B.atmp_re;

          V[cartesian_waypoints_planner_B.k_m].im *=

            cartesian_waypoints_planner_B.atmp_re;

          V[cartesian_waypoints_planner_B.i_p5].re *=

            cartesian_waypoints_planner_B.atmp_re;

          V[cartesian_waypoints_planner_B.i_p5].im *=

            cartesian_waypoints_planner_B.atmp_re;

          V[cartesian_waypoints_planner_B.jcol].re *=

            cartesian_waypoints_planner_B.atmp_re;

          V[cartesian_waypoints_planner_B.jcol].im *=

            cartesian_waypoints_planner_B.atmp_re;

        }

      }



      if (ilascl) {

        ilascl = true;

        while (ilascl) {

          cartesian_waypoints_planner_B.atmp_re =

            cartesian_waypoints_planner_B.anrmto * 2.0041683600089728E-292;

          cartesian_waypoints_planner_B.atmp_im =

            cartesian_waypoints_planner_B.anrm / 4.9896007738368E+291;

          if ((fabs(cartesian_waypoints_planner_B.atmp_re) > fabs

               (cartesian_waypoints_planner_B.anrm)) &&

              (cartesian_waypoints_planner_B.anrm != 0.0)) {

            cartesian_waypoints_planner_B.mul = 2.0041683600089728E-292;

            cartesian_waypoints_planner_B.anrmto =

              cartesian_waypoints_planner_B.atmp_re;

          } else if (fabs(cartesian_waypoints_planner_B.atmp_im) > fabs

                     (cartesian_waypoints_planner_B.anrmto)) {

            cartesian_waypoints_planner_B.mul = 4.9896007738368E+291;

            cartesian_waypoints_planner_B.anrm =

              cartesian_waypoints_planner_B.atmp_im;

          } else {

            cartesian_waypoints_planner_B.mul =

              cartesian_waypoints_planner_B.anrm /

              cartesian_waypoints_planner_B.anrmto;

            ilascl = false;

          }



          alpha1[0].re *= cartesian_waypoints_planner_B.mul;

          alpha1[0].im *= cartesian_waypoints_planner_B.mul;

          alpha1[1].re *= cartesian_waypoints_planner_B.mul;

          alpha1[1].im *= cartesian_waypoints_planner_B.mul;

          alpha1[2].re *= cartesian_waypoints_planner_B.mul;

          alpha1[2].im *= cartesian_waypoints_planner_B.mul;

          alpha1[3].re *= cartesian_waypoints_planner_B.mul;

          alpha1[3].im *= cartesian_waypoints_planner_B.mul;

        }

      }

    }

  }

}



// Function for MATLAB Function: '<S9>/MATLAB Function5'

static real_T cartesian_waypoints_pla_xnrm2_c(int32_T n, const real_T x[16],

  int32_T ix0)

{

  real_T y;

  real_T scale;

  real_T absxk;

  real_T t;

  int32_T k;

  y = 0.0;

  if (n >= 1) {

    if (n == 1) {

      y = fabs(x[ix0 - 1]);

    } else {

      scale = 3.3121686421112381E-170;

      for (k = ix0; k <= ix0 + 1; k++) {

        absxk = fabs(x[k - 1]);

        if (absxk > scale) {

          t = scale / absxk;

          y = y * t * t + 1.0;

          scale = absxk;

        } else {

          t = absxk / scale;

          y += t * t;

        }

      }



      y = scale * sqrt(y);

    }

  }



  return y;

}



// Function for MATLAB Function: '<S9>/MATLAB Function5'

static void cartesian_waypoints_plan_xzlarf(int32_T m, int32_T n, int32_T iv0,

  real_T tau, real_T C[16], int32_T ic0, real_T work[4])

{

  int32_T lastv;

  int32_T lastc;

  int32_T coltop;

  int32_T ix;

  real_T c;

  int32_T iac;

  int32_T d;

  int32_T b_ia;

  int32_T jy;

  int32_T exitg1;

  boolean_T exitg2;

  if (tau != 0.0) {

    lastv = m;

    lastc = iv0 + m;

    while ((lastv > 0) && (C[lastc - 2] == 0.0)) {

      lastv--;

      lastc--;

    }



    lastc = n - 1;

    exitg2 = false;

    while ((!exitg2) && (lastc + 1 > 0)) {

      coltop = (lastc << 2) + ic0;

      jy = coltop;

      do {

        exitg1 = 0;

        if (jy <= (coltop + lastv) - 1) {

          if (C[jy - 1] != 0.0) {

            exitg1 = 1;

          } else {

            jy++;

          }

        } else {

          lastc--;

          exitg1 = 2;

        }

      } while (exitg1 == 0);



      if (exitg1 == 1) {

        exitg2 = true;

      }

    }

  } else {

    lastv = 0;

    lastc = -1;

  }



  if (lastv > 0) {

    if (lastc + 1 != 0) {

      for (coltop = 0; coltop <= lastc; coltop++) {

        work[coltop] = 0.0;

      }



      coltop = 0;

      jy = (lastc << 2) + ic0;

      for (iac = ic0; iac <= jy; iac += 4) {

        ix = iv0;

        c = 0.0;

        d = (iac + lastv) - 1;

        for (b_ia = iac; b_ia <= d; b_ia++) {

          c += C[b_ia - 1] * C[ix - 1];

          ix++;

        }



        work[coltop] += c;

        coltop++;

      }

    }



    if (!(-tau == 0.0)) {

      coltop = ic0 - 1;

      jy = 0;

      for (iac = 0; iac <= lastc; iac++) {

        if (work[jy] != 0.0) {

          c = work[jy] * -tau;

          ix = iv0;

          d = lastv + coltop;

          for (b_ia = coltop; b_ia < d; b_ia++) {

            C[b_ia] += C[ix - 1] * c;

            ix++;

          }

        }



        jy++;

        coltop += 4;

      }

    }

  }

}



// Function for MATLAB Function: '<S9>/MATLAB Function5'

static void cartesian_waypoints_plan_xgehrd(real_T a[16], real_T tau[3])

{

  real_T work[4];

  real_T alpha1;

  real_T xnorm;

  int32_T knt;

  int32_T lastc;

  int32_T rowleft;

  int32_T iac;

  int32_T g;

  int32_T b_ia;

  int32_T jy;

  int32_T b_ix;

  int32_T exitg1;

  boolean_T exitg2;

  work[0] = 0.0;

  work[1] = 0.0;

  work[2] = 0.0;

  work[3] = 0.0;

  alpha1 = a[1];

  tau[0] = 0.0;

  xnorm = cartesian_waypoints_pla_xnrm2_c(2, a, 3);

  if (xnorm != 0.0) {

    xnorm = cartesian_waypoin_rt_hypotd_snf(a[1], xnorm);

    if (a[1] >= 0.0) {

      xnorm = -xnorm;

    }



    if (fabs(xnorm) < 1.0020841800044864E-292) {

      knt = -1;

      do {

        knt++;

        for (lastc = 3; lastc < 5; lastc++) {

          a[lastc - 1] *= 9.9792015476736E+291;

        }



        xnorm *= 9.9792015476736E+291;

        alpha1 *= 9.9792015476736E+291;

      } while (!(fabs(xnorm) >= 1.0020841800044864E-292));



      xnorm = cartesian_waypoin_rt_hypotd_snf(alpha1,

        cartesian_waypoints_pla_xnrm2_c(2, a, 3));

      if (alpha1 >= 0.0) {

        xnorm = -xnorm;

      }



      tau[0] = (xnorm - alpha1) / xnorm;

      alpha1 = 1.0 / (alpha1 - xnorm);

      for (lastc = 3; lastc < 5; lastc++) {

        a[lastc - 1] *= alpha1;

      }



      for (lastc = 0; lastc <= knt; lastc++) {

        xnorm *= 1.0020841800044864E-292;

      }



      alpha1 = xnorm;

    } else {

      tau[0] = (xnorm - a[1]) / xnorm;

      alpha1 = 1.0 / (a[1] - xnorm);

      for (knt = 3; knt < 5; knt++) {

        a[knt - 1] *= alpha1;

      }



      alpha1 = xnorm;

    }

  }



  a[1] = 1.0;

  if (tau[0] != 0.0) {

    knt = 2;

    lastc = 3;

    while ((knt + 1 > 0) && (a[lastc] == 0.0)) {

      knt--;

      lastc--;

    }



    lastc = 4;

    exitg2 = false;

    while ((!exitg2) && (lastc > 0)) {

      rowleft = lastc + 4;

      jy = rowleft;

      do {

        exitg1 = 0;

        if (jy <= (knt << 2) + rowleft) {

          if (a[jy - 1] != 0.0) {

            exitg1 = 1;

          } else {

            jy += 4;

          }

        } else {

          lastc--;

          exitg1 = 2;

        }

      } while (exitg1 == 0);



      if (exitg1 == 1) {

        exitg2 = true;

      }

    }

  } else {

    knt = -1;

    lastc = 0;

  }



  if (knt + 1 > 0) {

    if (lastc != 0) {

      for (rowleft = 0; rowleft < lastc; rowleft++) {

        work[rowleft] = 0.0;

      }



      rowleft = 1;

      jy = (knt << 2) + 5;

      for (iac = 5; iac <= jy; iac += 4) {

        b_ix = 0;

        g = (iac + lastc) - 1;

        for (b_ia = iac; b_ia <= g; b_ia++) {

          work[b_ix] += a[b_ia - 1] * a[rowleft];

          b_ix++;

        }



        rowleft++;

      }

    }



    if (!(-tau[0] == 0.0)) {

      rowleft = 4;

      jy = 1;

      for (iac = 0; iac <= knt; iac++) {

        if (a[jy] != 0.0) {

          xnorm = a[jy] * -tau[0];

          b_ix = 0;

          g = lastc + rowleft;

          for (b_ia = rowleft; b_ia < g; b_ia++) {

            a[b_ia] += work[b_ix] * xnorm;

            b_ix++;

          }

        }



        jy++;

        rowleft += 4;

      }

    }

  }



  cartesian_waypoints_plan_xzlarf(3, 3, 2, tau[0], a, 6, work);

  a[1] = alpha1;

  alpha1 = a[6];

  tau[1] = 0.0;

  xnorm = cartesian_waypoints_pla_xnrm2_c(1, a, 8);

  if (xnorm != 0.0) {

    xnorm = cartesian_waypoin_rt_hypotd_snf(a[6], xnorm);

    if (a[6] >= 0.0) {

      xnorm = -xnorm;

    }



    if (fabs(xnorm) < 1.0020841800044864E-292) {

      knt = -1;

      do {

        knt++;

        a[7] *= 9.9792015476736E+291;

        xnorm *= 9.9792015476736E+291;

        alpha1 *= 9.9792015476736E+291;

      } while (!(fabs(xnorm) >= 1.0020841800044864E-292));



      xnorm = cartesian_waypoin_rt_hypotd_snf(alpha1,

        cartesian_waypoints_pla_xnrm2_c(1, a, 8));

      if (alpha1 >= 0.0) {

        xnorm = -xnorm;

      }



      tau[1] = (xnorm - alpha1) / xnorm;

      alpha1 = 1.0 / (alpha1 - xnorm);

      a[7] *= alpha1;

      for (lastc = 0; lastc <= knt; lastc++) {

        xnorm *= 1.0020841800044864E-292;

      }



      alpha1 = xnorm;

    } else {

      tau[1] = (xnorm - a[6]) / xnorm;

      a[7] *= 1.0 / (a[6] - xnorm);

      alpha1 = xnorm;

    }

  }



  a[6] = 1.0;

  if (tau[1] != 0.0) {

    knt = 1;

    lastc = 7;

    while ((knt + 1 > 0) && (a[lastc] == 0.0)) {

      knt--;

      lastc--;

    }



    lastc = 4;

    exitg2 = false;

    while ((!exitg2) && (lastc > 0)) {

      rowleft = lastc + 8;

      jy = rowleft;

      do {

        exitg1 = 0;

        if (jy <= (knt << 2) + rowleft) {

          if (a[jy - 1] != 0.0) {

            exitg1 = 1;

          } else {

            jy += 4;

          }

        } else {

          lastc--;

          exitg1 = 2;

        }

      } while (exitg1 == 0);



      if (exitg1 == 1) {

        exitg2 = true;

      }

    }

  } else {

    knt = -1;

    lastc = 0;

  }



  if (knt + 1 > 0) {

    if (lastc != 0) {

      for (rowleft = 0; rowleft < lastc; rowleft++) {

        work[rowleft] = 0.0;

      }



      rowleft = 6;

      jy = (knt << 2) + 9;

      for (iac = 9; iac <= jy; iac += 4) {

        b_ix = 0;

        g = (iac + lastc) - 1;

        for (b_ia = iac; b_ia <= g; b_ia++) {

          work[b_ix] += a[b_ia - 1] * a[rowleft];

          b_ix++;

        }



        rowleft++;

      }

    }



    if (!(-tau[1] == 0.0)) {

      rowleft = 8;

      jy = 6;

      for (iac = 0; iac <= knt; iac++) {

        if (a[jy] != 0.0) {

          xnorm = a[jy] * -tau[1];

          b_ix = 0;

          g = lastc + rowleft;

          for (b_ia = rowleft; b_ia < g; b_ia++) {

            a[b_ia] += work[b_ix] * xnorm;

            b_ix++;

          }

        }



        jy++;

        rowleft += 4;

      }

    }

  }



  cartesian_waypoints_plan_xzlarf(2, 2, 7, tau[1], a, 11, work);

  a[6] = alpha1;

  alpha1 = a[11];

  tau[2] = 0.0;

  xnorm = cartesian_waypoints_pla_xnrm2_c(0, a, 12);

  if (xnorm != 0.0) {

    xnorm = cartesian_waypoin_rt_hypotd_snf(a[11], xnorm);

    if (a[11] >= 0.0) {

      xnorm = -xnorm;

    }



    if (fabs(xnorm) < 1.0020841800044864E-292) {

      knt = -1;

      do {

        knt++;

        xnorm *= 9.9792015476736E+291;

        alpha1 *= 9.9792015476736E+291;

      } while (!(fabs(xnorm) >= 1.0020841800044864E-292));



      xnorm = cartesian_waypoin_rt_hypotd_snf(alpha1,

        cartesian_waypoints_pla_xnrm2_c(0, a, 12));

      if (alpha1 >= 0.0) {

        xnorm = -xnorm;

      }



      tau[2] = (xnorm - alpha1) / xnorm;

      for (lastc = 0; lastc <= knt; lastc++) {

        xnorm *= 1.0020841800044864E-292;

      }



      alpha1 = xnorm;

    } else {

      tau[2] = (xnorm - a[11]) / xnorm;

      alpha1 = xnorm;

    }

  }



  a[11] = 1.0;

  if (tau[2] != 0.0) {

    knt = 0;

    lastc = 11;

    while ((knt + 1 > 0) && (a[lastc] == 0.0)) {

      knt--;

      lastc--;

    }



    lastc = 4;

    exitg2 = false;

    while ((!exitg2) && (lastc > 0)) {

      rowleft = lastc + 12;

      jy = rowleft;

      do {

        exitg1 = 0;

        if (jy <= (knt << 2) + rowleft) {

          if (a[jy - 1] != 0.0) {

            exitg1 = 1;

          } else {

            jy += 4;

          }

        } else {

          lastc--;

          exitg1 = 2;

        }

      } while (exitg1 == 0);



      if (exitg1 == 1) {

        exitg2 = true;

      }

    }

  } else {

    knt = -1;

    lastc = 0;

  }



  if (knt + 1 > 0) {

    if (lastc != 0) {

      for (rowleft = 0; rowleft < lastc; rowleft++) {

        work[rowleft] = 0.0;

      }



      rowleft = 11;

      jy = (knt << 2) + 13;

      for (iac = 13; iac <= jy; iac += 4) {

        b_ix = 0;

        g = (iac + lastc) - 1;

        for (b_ia = iac; b_ia <= g; b_ia++) {

          work[b_ix] += a[b_ia - 1] * a[rowleft];

          b_ix++;

        }



        rowleft++;

      }

    }



    if (!(-tau[2] == 0.0)) {

      rowleft = 12;

      jy = 11;

      for (iac = 0; iac <= knt; iac++) {

        if (a[jy] != 0.0) {

          xnorm = a[jy] * -tau[2];

          b_ix = 0;

          g = lastc + rowleft;

          for (b_ia = rowleft; b_ia < g; b_ia++) {

            a[b_ia] += work[b_ix] * xnorm;

            b_ix++;

          }

        }



        jy++;

        rowleft += 4;

      }

    }

  }



  cartesian_waypoints_plan_xzlarf(1, 1, 12, tau[2], a, 16, work);

  a[11] = alpha1;

}



// Function for MATLAB Function: '<S9>/MATLAB Function5'

static real_T cartesian_waypoints_pl_xnrm2_cc(int32_T n, const real_T x[3])

{

  real_T y;

  real_T scale;

  real_T absxk;

  real_T t;

  y = 0.0;

  if (n >= 1) {

    if (n == 1) {

      y = fabs(x[1]);

    } else {

      scale = 3.3121686421112381E-170;

      absxk = fabs(x[1]);

      if (absxk > 3.3121686421112381E-170) {

        y = 1.0;

        scale = absxk;

      } else {

        t = absxk / 3.3121686421112381E-170;

        y = t * t;

      }



      absxk = fabs(x[2]);

      if (absxk > scale) {

        t = scale / absxk;

        y = y * t * t + 1.0;

        scale = absxk;

      } else {

        t = absxk / scale;

        y += t * t;

      }



      y = scale * sqrt(y);

    }

  }



  return y;

}



// Function for MATLAB Function: '<S9>/MATLAB Function5'

static real_T cartesian_waypoints_pla_xzlarfg(int32_T n, real_T *alpha1, real_T

  x[3])

{

  real_T tau;

  real_T xnorm;

  int32_T knt;

  int32_T c_k;

  tau = 0.0;

  if (n > 0) {

    xnorm = cartesian_waypoints_pl_xnrm2_cc(n - 1, x);

    if (xnorm != 0.0) {

      xnorm = cartesian_waypoin_rt_hypotd_snf(*alpha1, xnorm);

      if (*alpha1 >= 0.0) {

        xnorm = -xnorm;

      }



      if (fabs(xnorm) < 1.0020841800044864E-292) {

        knt = -1;

        do {

          knt++;

          for (c_k = 1; c_k < n; c_k++) {

            x[c_k] *= 9.9792015476736E+291;

          }



          xnorm *= 9.9792015476736E+291;

          *alpha1 *= 9.9792015476736E+291;

        } while (!(fabs(xnorm) >= 1.0020841800044864E-292));



        xnorm = cartesian_waypoin_rt_hypotd_snf(*alpha1,

          cartesian_waypoints_pl_xnrm2_cc(n - 1, x));

        if (*alpha1 >= 0.0) {

          xnorm = -xnorm;

        }



        tau = (xnorm - *alpha1) / xnorm;

        *alpha1 = 1.0 / (*alpha1 - xnorm);

        for (c_k = 1; c_k < n; c_k++) {

          x[c_k] *= *alpha1;

        }



        for (c_k = 0; c_k <= knt; c_k++) {

          xnorm *= 1.0020841800044864E-292;

        }



        *alpha1 = xnorm;

      } else {

        tau = (xnorm - *alpha1) / xnorm;

        *alpha1 = 1.0 / (*alpha1 - xnorm);

        for (knt = 1; knt < n; knt++) {

          x[knt] *= *alpha1;

        }



        *alpha1 = xnorm;

      }

    }

  }



  return tau;

}



// Function for MATLAB Function: '<S9>/MATLAB Function5'

static void cartesian_waypoints_pla_xdlanv2(real_T *a, real_T *b, real_T *c,

  real_T *d, real_T *rt1r, real_T *rt1i, real_T *rt2r, real_T *rt2i, real_T *cs,

  real_T *sn)

{

  real_T p;

  real_T bcmax;

  real_T bcmis;

  real_T scale;

  real_T z;

  real_T tau;

  int32_T b_0;

  int32_T c_0;

  boolean_T tmp;

  if (*c == 0.0) {

    *cs = 1.0;

    *sn = 0.0;

  } else if (*b == 0.0) {

    *cs = 0.0;

    *sn = 1.0;

    bcmax = *d;

    *d = *a;

    *a = bcmax;

    *b = -*c;

    *c = 0.0;

  } else {

    tau = *a - *d;

    if ((tau == 0.0) && ((*b < 0.0) != (*c < 0.0))) {

      *cs = 1.0;

      *sn = 0.0;

    } else {

      p = 0.5 * tau;

      bcmis = fabs(*b);

      z = fabs(*c);

      tmp = rtIsNaN(z);

      if ((bcmis > z) || tmp) {

        bcmax = bcmis;

      } else {

        bcmax = z;

      }



      if ((bcmis < z) || tmp) {

        z = bcmis;

      }



      if (!(*b < 0.0)) {

        b_0 = 1;

      } else {

        b_0 = -1;

      }



      if (!(*c < 0.0)) {

        c_0 = 1;

      } else {

        c_0 = -1;

      }



      bcmis = z * static_cast<real_T>(b_0) * static_cast<real_T>(c_0);

      scale = fabs(p);

      if ((!(scale > bcmax)) && (!rtIsNaN(bcmax))) {

        scale = bcmax;

      }



      z = p / scale * p + bcmax / scale * bcmis;

      if (z >= 8.8817841970012523E-16) {

        if (!(p < 0.0)) {

          tau = sqrt(scale) * sqrt(z);

        } else {

          tau = -(sqrt(scale) * sqrt(z));

        }



        z = p + tau;

        *a = *d + z;

        *d -= bcmax / z * bcmis;

        tau = cartesian_waypoin_rt_hypotd_snf(*c, z);

        *cs = z / tau;

        *sn = *c / tau;

        *b -= *c;

        *c = 0.0;

      } else {

        bcmax = *b + *c;

        tau = cartesian_waypoin_rt_hypotd_snf(bcmax, tau);

        *cs = sqrt((fabs(bcmax) / tau + 1.0) * 0.5);

        if (!(bcmax < 0.0)) {

          b_0 = 1;

        } else {

          b_0 = -1;

        }



        *sn = -(p / (tau * *cs)) * static_cast<real_T>(b_0);

        p = *a * *cs + *b * *sn;

        tau = -*a * *sn + *b * *cs;

        bcmax = *c * *cs + *d * *sn;

        bcmis = -*c * *sn + *d * *cs;

        *b = tau * *cs + bcmis * *sn;

        *c = -p * *sn + bcmax * *cs;

        bcmax = ((p * *cs + bcmax * *sn) + (-tau * *sn + bcmis * *cs)) * 0.5;

        *a = bcmax;

        *d = bcmax;

        if (*c != 0.0) {

          if (*b != 0.0) {

            if ((*b < 0.0) == (*c < 0.0)) {

              z = sqrt(fabs(*b));

              bcmis = sqrt(fabs(*c));

              if (!(*c < 0.0)) {

                p = z * bcmis;

              } else {

                p = -(z * bcmis);

              }



              tau = 1.0 / sqrt(fabs(*b + *c));

              *a = bcmax + p;

              *d = bcmax - p;

              *b -= *c;

              *c = 0.0;

              p = z * tau;

              tau *= bcmis;

              bcmax = *cs * p - *sn * tau;

              *sn = *cs * tau + *sn * p;

              *cs = bcmax;

            }

          } else {

            *b = -*c;

            *c = 0.0;

            bcmax = *cs;

            *cs = -*sn;

            *sn = bcmax;

          }

        }

      }

    }

  }



  *rt1r = *a;

  *rt2r = *d;

  if (*c == 0.0) {

    *rt1i = 0.0;

    *rt2i = 0.0;

  } else {

    *rt1i = sqrt(fabs(*b)) * sqrt(fabs(*c));

    *rt2i = -*rt1i;

  }

}



// Function for MATLAB Function: '<S9>/MATLAB Function5'

static void cartesian_waypoints_plan_xrot_l(int32_T n, real_T x[16], int32_T ix0,

  int32_T iy0, real_T c, real_T s)

{

  int32_T ix;

  int32_T iy;

  real_T temp;

  int32_T k;

  ix = ix0 - 1;

  iy = iy0 - 1;

  for (k = 0; k < n; k++) {

    temp = c * x[ix] + s * x[iy];

    x[iy] = c * x[iy] - s * x[ix];

    x[ix] = temp;

    iy += 4;

    ix += 4;

  }

}



// Function for MATLAB Function: '<S9>/MATLAB Function5'

static void cartesian_waypoints_pla_xrot_lr(int32_T n, real_T x[16], int32_T ix0,

  int32_T iy0, real_T c, real_T s)

{

  int32_T ix;

  int32_T iy;

  real_T temp;

  int32_T k;

  if (n >= 1) {

    ix = ix0 - 1;

    iy = iy0 - 1;

    for (k = 0; k < n; k++) {

      temp = c * x[ix] + s * x[iy];

      x[iy] = c * x[iy] - s * x[ix];

      x[ix] = temp;

      iy++;

      ix++;

    }

  }

}



// Function for MATLAB Function: '<S9>/MATLAB Function5'

static int32_T cartesian_waypoints__eml_dlahqr(real_T h[16], real_T z[16])

{

  int32_T info;

  real_T v[3];

  int32_T i;

  int32_T L;

  boolean_T goto150;

  int32_T k;

  real_T tst;

  real_T htmp1;

  real_T htmp2;

  real_T ba;

  real_T aa;

  real_T h12;

  int32_T m;

  real_T h21s;

  int32_T nr;

  int32_T hoffset;

  real_T unusedU1;

  real_T unusedU2;

  real_T unusedU3;

  real_T b_v[3];

  int32_T j;

  int32_T b_j;

  int32_T c_j;

  int32_T ix;

  int32_T s_tmp;

  boolean_T exitg1;

  boolean_T exitg2;

  boolean_T exitg3;

  info = 0;

  v[0] = 0.0;

  v[1] = 0.0;

  v[2] = 0.0;

  h[2] = 0.0;

  h[3] = 0.0;

  h[7] = 0.0;

  i = 3;

  exitg1 = false;

  while ((!exitg1) && (i + 1 >= 1)) {

    L = 1;

    goto150 = false;

    ix = 0;

    exitg2 = false;

    while ((!exitg2) && (ix < 301)) {

      k = i;

      exitg3 = false;

      while ((!exitg3) && (k + 1 > L)) {

        s_tmp = ((k - 1) << 2) + k;

        htmp2 = fabs(h[s_tmp]);

        if (htmp2 <= 4.0083367200179456E-292) {

          exitg3 = true;

        } else {

          m = (k << 2) + k;

          nr = s_tmp - 1;

          tst = fabs(h[nr]) + fabs(h[m]);

          if (tst == 0.0) {

            if (k - 1 >= 1) {

              tst = fabs(h[(((k - 2) << 2) + k) - 1]);

            }



            if (k + 2 <= 4) {

              tst += fabs(h[((k << 2) + k) + 1]);

            }

          }



          if (htmp2 <= 2.2204460492503131E-16 * tst) {

            htmp1 = fabs(h[s_tmp]);

            htmp2 = fabs(h[m - 1]);

            if (htmp1 > htmp2) {

              tst = htmp1;

              ba = htmp2;

            } else {

              tst = htmp2;

              ba = htmp1;

            }



            htmp1 = fabs(h[m]);

            htmp2 = fabs(h[nr] - h[m]);

            if (htmp1 > htmp2) {

              aa = htmp1;

              htmp1 = htmp2;

            } else {

              aa = htmp2;

            }



            htmp2 = aa + tst;

            htmp1 = aa / htmp2 * htmp1 * 2.2204460492503131E-16;

            if ((4.0083367200179456E-292 > htmp1) || rtIsNaN(htmp1)) {

              htmp1 = 4.0083367200179456E-292;

            }



            if (tst / htmp2 * ba <= htmp1) {

              exitg3 = true;

            } else {

              k--;

            }

          } else {

            k--;

          }

        }

      }



      L = k + 1;

      if (k + 1 > 1) {

        h[k + ((k - 1) << 2)] = 0.0;

      }



      if (k + 1 >= i) {

        goto150 = true;

        exitg2 = true;

      } else {

        switch (ix) {

         case 10:

          s_tmp = (k << 2) + k;

          htmp2 = fabs(h[(((k + 1) << 2) + k) + 2]) + fabs(h[s_tmp + 1]);

          ba = h[s_tmp] + 0.75 * htmp2;

          h12 = -0.4375 * htmp2;

          aa = htmp2;

          tst = ba;

          break;



         case 20:

          htmp2 = fabs(h[(((i - 2) << 2) + i) - 1]) + fabs(h[((i - 1) << 2) + i]);

          ba = h[(i << 2) + i] + 0.75 * htmp2;

          h12 = -0.4375 * htmp2;

          aa = htmp2;

          tst = ba;

          break;



         default:

          ba = h[(((i - 1) << 2) + i) - 1];

          aa = h[((i - 1) << 2) + i];

          h12 = h[((i << 2) + i) - 1];

          tst = h[(i << 2) + i];

          break;

        }



        htmp2 = ((fabs(ba) + fabs(h12)) + fabs(aa)) + fabs(tst);

        if (htmp2 == 0.0) {

          ba = 0.0;

          tst = 0.0;

          htmp1 = 0.0;

          aa = 0.0;

        } else {

          ba /= htmp2;

          aa /= htmp2;

          h12 /= htmp2;

          tst /= htmp2;

          htmp1 = (ba + tst) / 2.0;

          ba = (ba - htmp1) * (tst - htmp1) - h12 * aa;

          aa = sqrt(fabs(ba));

          if (ba >= 0.0) {

            ba = htmp1 * htmp2;

            htmp1 = ba;

            tst = aa * htmp2;

            aa = -tst;

          } else {

            ba = htmp1 + aa;

            htmp1 -= aa;

            if (fabs(ba - tst) <= fabs(htmp1 - tst)) {

              ba *= htmp2;

              htmp1 = ba;

            } else {

              htmp1 *= htmp2;

              ba = htmp1;

            }



            tst = 0.0;

            aa = 0.0;

          }

        }



        m = i - 1;

        exitg3 = false;

        while ((!exitg3) && (m >= k + 1)) {

          s_tmp = ((m - 1) << 2) + m;

          nr = s_tmp - 1;

          h12 = h[nr] - htmp1;

          htmp2 = (fabs(h12) + fabs(aa)) + fabs(h[s_tmp]);

          h21s = h[s_tmp] / htmp2;

          hoffset = (m << 2) + m;

          v[0] = (h12 / htmp2 * (h[nr] - ba) + h[hoffset - 1] * h21s) - aa /

            htmp2 * tst;

          v[1] = (((h[nr] + h[hoffset]) - ba) - htmp1) * h21s;

          v[2] = h[hoffset + 1] * h21s;

          htmp2 = (fabs(v[0]) + fabs(v[1])) + fabs(v[2]);

          v[0] /= htmp2;

          v[1] /= htmp2;

          v[2] /= htmp2;

          if (k + 1 == m) {

            exitg3 = true;

          } else {

            s_tmp = ((m - 2) << 2) + m;

            if (fabs(h[s_tmp - 1]) * (fabs(v[1]) + fabs(v[2])) <= ((fabs(h[s_tmp

                   - 2]) + fabs(h[nr])) + fabs(h[hoffset])) *

                (2.2204460492503131E-16 * fabs(v[0]))) {

              exitg3 = true;

            } else {

              m--;

            }

          }

        }



        for (s_tmp = m; s_tmp <= i; s_tmp++) {

          nr = (i - s_tmp) + 2;

          if (3 < nr) {

            nr = 3;

          }



          if (s_tmp > m) {

            hoffset = ((s_tmp - 2) << 2) + s_tmp;

            for (j = 0; j < nr; j++) {

              v[j] = h[(j + hoffset) - 1];

            }

          }



          tst = v[0];

          b_v[0] = v[0];

          b_v[1] = v[1];

          b_v[2] = v[2];

          htmp2 = cartesian_waypoints_pla_xzlarfg(nr, &tst, b_v);

          v[1] = b_v[1];

          v[2] = b_v[2];

          v[0] = tst;

          if (s_tmp > m) {

            h[(s_tmp + ((s_tmp - 2) << 2)) - 1] = tst;

            h[s_tmp + ((s_tmp - 2) << 2)] = 0.0;

            if (s_tmp < i) {

              h[s_tmp + 1] = 0.0;

            }

          } else {

            if (m > k + 1) {

              h[s_tmp - 1] *= 1.0 - htmp2;

            }

          }



          tst = b_v[1];

          ba = htmp2 * b_v[1];

          switch (nr) {

           case 3:

            htmp1 = b_v[2];

            aa = htmp2 * b_v[2];

            for (b_j = s_tmp - 1; b_j + 1 < 5; b_j++) {

              nr = (b_j << 2) + s_tmp;

              hoffset = nr - 1;

              j = nr + 1;

              h12 = (h[hoffset] + h[nr] * tst) + h[j] * htmp1;

              h[hoffset] -= h12 * htmp2;

              h[nr] -= h12 * ba;

              h[j] -= h12 * aa;

            }



            nr = s_tmp + 3;

            b_j = i + 1;

            if (nr < b_j) {

              b_j = nr;

            }



            for (c_j = 0; c_j < b_j; c_j++) {

              nr = ((s_tmp - 1) << 2) + c_j;

              hoffset = (s_tmp << 2) + c_j;

              j = ((s_tmp + 1) << 2) + c_j;

              h12 = (h[nr] + h[hoffset] * tst) + h[j] * htmp1;

              h[nr] -= h12 * htmp2;

              h[hoffset] -= h12 * ba;

              h[j] -= h12 * aa;

            }



            for (b_j = 0; b_j < 4; b_j++) {

              nr = ((s_tmp - 1) << 2) + b_j;

              hoffset = (s_tmp << 2) + b_j;

              j = ((s_tmp + 1) << 2) + b_j;

              h12 = (z[nr] + z[hoffset] * tst) + z[j] * htmp1;

              z[nr] -= h12 * htmp2;

              z[hoffset] -= h12 * ba;

              z[j] -= h12 * aa;

            }

            break;



           case 2:

            for (j = s_tmp - 1; j + 1 < 5; j++) {

              nr = (j << 2) + s_tmp;

              hoffset = nr - 1;

              h12 = h[hoffset] + h[nr] * tst;

              h[hoffset] -= h12 * htmp2;

              h[nr] -= h12 * ba;

            }



            for (j = 0; j <= i; j++) {

              nr = ((s_tmp - 1) << 2) + j;

              hoffset = (s_tmp << 2) + j;

              h12 = h[nr] + h[hoffset] * tst;

              h[nr] -= h12 * htmp2;

              h[hoffset] -= h12 * ba;

            }



            for (j = 0; j < 4; j++) {

              nr = ((s_tmp - 1) << 2) + j;

              hoffset = (s_tmp << 2) + j;

              h12 = z[nr] + z[hoffset] * tst;

              z[nr] -= h12 * htmp2;

              z[hoffset] -= h12 * ba;

            }

            break;

          }

        }



        ix++;

      }

    }



    if (!goto150) {

      info = i + 1;

      exitg1 = true;

    } else {

      if ((i + 1 != L) && (L == i)) {

        ix = (i - 1) << 2;

        k = ix + i;

        m = k - 1;

        ba = h[m];

        s_tmp = i << 2;

        nr = s_tmp + i;

        hoffset = nr - 1;

        htmp1 = h[hoffset];

        aa = h[k];

        h12 = h[nr];

        cartesian_waypoints_pla_xdlanv2(&ba, &htmp1, &aa, &h12, &h21s, &unusedU1,

          &unusedU2, &unusedU3, &htmp2, &tst);

        h[m] = ba;

        h[hoffset] = htmp1;

        h[k] = aa;

        h[nr] = h12;

        if (4 > i + 1) {

          cartesian_waypoints_plan_xrot_l(3 - i, h, i + ((i + 1) << 2), (i + ((i

            + 1) << 2)) + 1, htmp2, tst);

        }



        cartesian_waypoints_pla_xrot_lr(i - 1, h, ((i - 1) << 2) + 1, (i << 2) +

          1, htmp2, tst);

        ba = htmp2 * z[ix] + tst * z[s_tmp];

        z[s_tmp] = htmp2 * z[s_tmp] - tst * z[ix];

        z[ix] = ba;

        i = s_tmp + 1;

        ix++;

        ba = htmp2 * z[ix] + tst * z[i];

        z[i] = htmp2 * z[i] - tst * z[ix];

        z[ix] = ba;

        i++;

        ix++;

        ba = htmp2 * z[ix] + tst * z[i];

        z[i] = htmp2 * z[i] - tst * z[ix];

        z[ix] = ba;

        i++;

        ix++;

        ba = htmp2 * z[ix] + tst * z[i];

        z[i] = htmp2 * z[i] - tst * z[ix];

        z[ix] = ba;

      }



      i = L - 2;

    }

  }



  return info;

}



// Function for MATLAB Function: '<S9>/MATLAB Function5'

static void cartesian_waypoints_planner_eig(const real_T A[16], creal_T V[16],

  creal_T D[4])

{

  boolean_T p;

  int32_T exitg1;

  boolean_T exitg2;

  if (cartesian_waypoint_anyNonFinite(A)) {

    for (cartesian_waypoints_planner_B.b_j_k = 0;

         cartesian_waypoints_planner_B.b_j_k < 16;

         cartesian_waypoints_planner_B.b_j_k++) {

      V[cartesian_waypoints_planner_B.b_j_k].re = (rtNaN);

      V[cartesian_waypoints_planner_B.b_j_k].im = 0.0;

    }



    D[0].re = (rtNaN);

    D[0].im = 0.0;

    D[1].re = (rtNaN);

    D[1].im = 0.0;

    D[2].re = (rtNaN);

    D[2].im = 0.0;

    D[3].re = (rtNaN);

    D[3].im = 0.0;

  } else {

    p = true;

    cartesian_waypoints_planner_B.b_j_k = 0;

    exitg2 = false;

    while ((!exitg2) && (cartesian_waypoints_planner_B.b_j_k < 4)) {

      cartesian_waypoints_planner_B.i_p = 0;

      do {

        exitg1 = 0;

        if (cartesian_waypoints_planner_B.i_p <=

            cartesian_waypoints_planner_B.b_j_k) {

          if (!(A[(cartesian_waypoints_planner_B.b_j_k << 2) +

                cartesian_waypoints_planner_B.i_p] == A

                [(cartesian_waypoints_planner_B.i_p << 2) +

                cartesian_waypoints_planner_B.b_j_k])) {

            p = false;

            exitg1 = 1;

          } else {

            cartesian_waypoints_planner_B.i_p++;

          }

        } else {

          cartesian_waypoints_planner_B.b_j_k++;

          exitg1 = 2;

        }

      } while (exitg1 == 0);



      if (exitg1 == 1) {

        exitg2 = true;

      }

    }



    if (p) {

      if (cartesian_waypoint_anyNonFinite(A)) {

        for (cartesian_waypoints_planner_B.b_j_k = 0;

             cartesian_waypoints_planner_B.b_j_k < 16;

             cartesian_waypoints_planner_B.b_j_k++) {

          cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k]

            = (rtNaN);

        }



        cartesian_waypoints_planner_B.b_j_k = 2;

        while (cartesian_waypoints_planner_B.b_j_k < 5) {

          cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k

            - 1] = 0.0;

          cartesian_waypoints_planner_B.b_j_k++;

        }



        cartesian_waypoints_planner_B.b_j_k = 3;

        while (cartesian_waypoints_planner_B.b_j_k < 5) {

          cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k

            + 3] = 0.0;

          cartesian_waypoints_planner_B.b_j_k++;

        }



        cartesian_waypoints_planner_B.b_V[11] = 0.0;

        for (cartesian_waypoints_planner_B.b_j_k = 0;

             cartesian_waypoints_planner_B.b_j_k < 16;

             cartesian_waypoints_planner_B.b_j_k++) {

          cartesian_waypoints_planner_B.b_A[cartesian_waypoints_planner_B.b_j_k]

            = (rtNaN);

        }

      } else {

        memcpy(&cartesian_waypoints_planner_B.b_A[0], &A[0], sizeof(real_T) <<

               4U);

        cartesian_waypoints_plan_xgehrd(cartesian_waypoints_planner_B.b_A,

          cartesian_waypoints_planner_B.tau);

        memcpy(&cartesian_waypoints_planner_B.b_V[0],

               &cartesian_waypoints_planner_B.b_A[0], sizeof(real_T) << 4U);

        cartesian_waypoints_planner_B.b_j_k = 0;

        while (cartesian_waypoints_planner_B.b_j_k <= 2) {

          cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k

            + 12] = 0.0;

          cartesian_waypoints_planner_B.b_j_k++;

        }



        cartesian_waypoints_planner_B.b_j_k = 0;

        while (cartesian_waypoints_planner_B.b_j_k <= 1) {

          cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k

            + 8] = 0.0;

          cartesian_waypoints_planner_B.b_j_k++;

        }



        cartesian_waypoints_planner_B.b_j_k = 1;

        while (cartesian_waypoints_planner_B.b_j_k + 3 < 5) {

          cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k

            + 10] =

            cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k

            + 6];

          cartesian_waypoints_planner_B.b_j_k++;

        }



        cartesian_waypoints_planner_B.b_V[4] = 0.0;

        cartesian_waypoints_planner_B.b_j_k = 0;

        while (cartesian_waypoints_planner_B.b_j_k + 3 < 5) {

          cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k

            + 6] =

            cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k

            + 2];

          cartesian_waypoints_planner_B.b_j_k++;

        }



        cartesian_waypoints_planner_B.work[0] = 0.0;

        cartesian_waypoints_planner_B.b_V[1] = 0.0;

        cartesian_waypoints_planner_B.work[1] = 0.0;

        cartesian_waypoints_planner_B.b_V[2] = 0.0;

        cartesian_waypoints_planner_B.work[2] = 0.0;

        cartesian_waypoints_planner_B.b_V[3] = 0.0;

        cartesian_waypoints_planner_B.work[3] = 0.0;

        cartesian_waypoints_planner_B.b_V[0] = 1.0;

        cartesian_waypoints_planner_B.b_V[15] = 1.0 -

          cartesian_waypoints_planner_B.tau[2];

        cartesian_waypoints_planner_B.b_j_k = 0;

        while (cartesian_waypoints_planner_B.b_j_k <= 1) {

          cartesian_waypoints_planner_B.b_V[14 -

            cartesian_waypoints_planner_B.b_j_k] = 0.0;

          cartesian_waypoints_planner_B.b_j_k++;

        }



        cartesian_waypoints_planner_B.b_V[10] = 1.0;

        cartesian_waypoints_plan_xzlarf(2, 1, 11,

          cartesian_waypoints_planner_B.tau[1],

          cartesian_waypoints_planner_B.b_V, 15,

          cartesian_waypoints_planner_B.work);

        cartesian_waypoints_planner_B.b_j_k = 11;

        while (cartesian_waypoints_planner_B.b_j_k + 1 <= 12) {

          cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k]

            *= -cartesian_waypoints_planner_B.tau[1];

          cartesian_waypoints_planner_B.b_j_k++;

        }



        cartesian_waypoints_planner_B.b_V[10] = 1.0 -

          cartesian_waypoints_planner_B.tau[1];

        cartesian_waypoints_planner_B.b_V[9] = 0.0;

        cartesian_waypoints_planner_B.b_V[5] = 1.0;

        cartesian_waypoints_plan_xzlarf(3, 2, 6,

          cartesian_waypoints_planner_B.tau[0],

          cartesian_waypoints_planner_B.b_V, 10,

          cartesian_waypoints_planner_B.work);

        cartesian_waypoints_planner_B.b_j_k = 6;

        while (cartesian_waypoints_planner_B.b_j_k + 1 <= 8) {

          cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k]

            *= -cartesian_waypoints_planner_B.tau[0];

          cartesian_waypoints_planner_B.b_j_k++;

        }



        cartesian_waypoints_planner_B.b_V[5] = 1.0 -

          cartesian_waypoints_planner_B.tau[0];

        cartesian_waypoints__eml_dlahqr(cartesian_waypoints_planner_B.b_A,

          cartesian_waypoints_planner_B.b_V);

      }



      for (cartesian_waypoints_planner_B.b_j_k = 0;

           cartesian_waypoints_planner_B.b_j_k < 16;

           cartesian_waypoints_planner_B.b_j_k++) {

        V[cartesian_waypoints_planner_B.b_j_k].re =

          cartesian_waypoints_planner_B.b_V[cartesian_waypoints_planner_B.b_j_k];

        V[cartesian_waypoints_planner_B.b_j_k].im = 0.0;

      }



      D[0].re = cartesian_waypoints_planner_B.b_A[0];

      D[0].im = 0.0;

      D[1].re = cartesian_waypoints_planner_B.b_A[5];

      D[1].im = 0.0;

      D[2].re = cartesian_waypoints_planner_B.b_A[10];

      D[2].im = 0.0;

      D[3].re = cartesian_waypoints_planner_B.b_A[15];

      D[3].im = 0.0;

    } else {

      for (cartesian_waypoints_planner_B.b_j_k = 0;

           cartesian_waypoints_planner_B.b_j_k < 16;

           cartesian_waypoints_planner_B.b_j_k++) {

        cartesian_waypoints_planner_B.At[cartesian_waypoints_planner_B.b_j_k].re

          = A[cartesian_waypoints_planner_B.b_j_k];

        cartesian_waypoints_planner_B.At[cartesian_waypoints_planner_B.b_j_k].im

          = 0.0;

      }



      cartesian_waypoints_plan_xzggev(cartesian_waypoints_planner_B.At,

        &cartesian_waypoints_planner_B.b_j_k, D,

        cartesian_waypoints_planner_B.beta1, V);

      cartesian_waypoints_planner_B.colnorm = 0.0;

      cartesian_waypoints_planner_B.scale_f = 3.3121686421112381E-170;

      cartesian_waypoints_planner_B.b_j_k = 0;

      while (cartesian_waypoints_planner_B.b_j_k + 1 <= 4) {

        cartesian_waypoints_planner_B.absxk = fabs

          (V[cartesian_waypoints_planner_B.b_j_k].re);

        if (cartesian_waypoints_planner_B.absxk >

            cartesian_waypoints_planner_B.scale_f) {

          cartesian_waypoints_planner_B.t_c =

            cartesian_waypoints_planner_B.scale_f /

            cartesian_waypoints_planner_B.absxk;

          cartesian_waypoints_planner_B.colnorm =

            cartesian_waypoints_planner_B.colnorm *

            cartesian_waypoints_planner_B.t_c *

            cartesian_waypoints_planner_B.t_c + 1.0;

          cartesian_waypoints_planner_B.scale_f =

            cartesian_waypoints_planner_B.absxk;

        } else {

          cartesian_waypoints_planner_B.t_c =

            cartesian_waypoints_planner_B.absxk /

            cartesian_waypoints_planner_B.scale_f;

          cartesian_waypoints_planner_B.colnorm +=

            cartesian_waypoints_planner_B.t_c *

            cartesian_waypoints_planner_B.t_c;

        }



        cartesian_waypoints_planner_B.absxk = fabs

          (V[cartesian_waypoints_planner_B.b_j_k].im);

        if (cartesian_waypoints_planner_B.absxk >

            cartesian_waypoints_planner_B.scale_f) {

          cartesian_waypoints_planner_B.t_c =

            cartesian_waypoints_planner_B.scale_f /

            cartesian_waypoints_planner_B.absxk;

          cartesian_waypoints_planner_B.colnorm =

            cartesian_waypoints_planner_B.colnorm *

            cartesian_waypoints_planner_B.t_c *

            cartesian_waypoints_planner_B.t_c + 1.0;

          cartesian_waypoints_planner_B.scale_f =

            cartesian_waypoints_planner_B.absxk;

        } else {

          cartesian_waypoints_planner_B.t_c =

            cartesian_waypoints_planner_B.absxk /

            cartesian_waypoints_planner_B.scale_f;

          cartesian_waypoints_planner_B.colnorm +=

            cartesian_waypoints_planner_B.t_c *

            cartesian_waypoints_planner_B.t_c;

        }



        cartesian_waypoints_planner_B.b_j_k++;

      }



      cartesian_waypoints_planner_B.colnorm =

        cartesian_waypoints_planner_B.scale_f * sqrt

        (cartesian_waypoints_planner_B.colnorm);

      cartesian_waypoints_planner_B.b_j_k = 0;

      while (cartesian_waypoints_planner_B.b_j_k + 1 <= 4) {

        if (V[cartesian_waypoints_planner_B.b_j_k].im == 0.0) {

          cartesian_waypoints_planner_B.scale_f =

            V[cartesian_waypoints_planner_B.b_j_k].re /

            cartesian_waypoints_planner_B.colnorm;

          cartesian_waypoints_planner_B.absxk = 0.0;

        } else if (V[cartesian_waypoints_planner_B.b_j_k].re == 0.0) {

          cartesian_waypoints_planner_B.scale_f = 0.0;

          cartesian_waypoints_planner_B.absxk =

            V[cartesian_waypoints_planner_B.b_j_k].im /

            cartesian_waypoints_planner_B.colnorm;

        } else {

          cartesian_waypoints_planner_B.scale_f =

            V[cartesian_waypoints_planner_B.b_j_k].re /

            cartesian_waypoints_planner_B.colnorm;

          cartesian_waypoints_planner_B.absxk =

            V[cartesian_waypoints_planner_B.b_j_k].im /

            cartesian_waypoints_planner_B.colnorm;

        }



        V[cartesian_waypoints_planner_B.b_j_k].re =

          cartesian_waypoints_planner_B.scale_f;

        V[cartesian_waypoints_planner_B.b_j_k].im =

          cartesian_waypoints_planner_B.absxk;

        cartesian_waypoints_planner_B.b_j_k++;

      }



      cartesian_waypoints_planner_B.colnorm = 0.0;

      cartesian_waypoints_planner_B.scale_f = 3.3121686421112381E-170;

      cartesian_waypoints_planner_B.b_j_k = 4;

      while (cartesian_waypoints_planner_B.b_j_k + 1 <= 8) {

        cartesian_waypoints_planner_B.absxk = fabs

          (V[cartesian_waypoints_planner_B.b_j_k].re);

        if (cartesian_waypoints_planner_B.absxk >

            cartesian_waypoints_planner_B.scale_f) {

          cartesian_waypoints_planner_B.t_c =

            cartesian_waypoints_planner_B.scale_f /

            cartesian_waypoints_planner_B.absxk;

          cartesian_waypoints_planner_B.colnorm =

            cartesian_waypoints_planner_B.colnorm *

            cartesian_waypoints_planner_B.t_c *

            cartesian_waypoints_planner_B.t_c + 1.0;

          cartesian_waypoints_planner_B.scale_f =

            cartesian_waypoints_planner_B.absxk;

        } else {

          cartesian_waypoints_planner_B.t_c =

            cartesian_waypoints_planner_B.absxk /

            cartesian_waypoints_planner_B.scale_f;

          cartesian_waypoints_planner_B.colnorm +=

            cartesian_waypoints_planner_B.t_c *

            cartesian_waypoints_planner_B.t_c;

        }



        cartesian_waypoints_planner_B.absxk = fabs

          (V[cartesian_waypoints_planner_B.b_j_k].im);

        if (cartesian_waypoints_planner_B.absxk >

            cartesian_waypoints_planner_B.scale_f) {

          cartesian_waypoints_planner_B.t_c =

            cartesian_waypoints_planner_B.scale_f /

            cartesian_waypoints_planner_B.absxk;

          cartesian_waypoints_planner_B.colnorm =

            cartesian_waypoints_planner_B.colnorm *

            cartesian_waypoints_planner_B.t_c *

            cartesian_waypoints_planner_B.t_c + 1.0;

          cartesian_waypoints_planner_B.scale_f =

            cartesian_waypoints_planner_B.absxk;

        } else {

          cartesian_waypoints_planner_B.t_c =

            cartesian_waypoints_planner_B.absxk /

            cartesian_waypoints_planner_B.scale_f;

          cartesian_waypoints_planner_B.colnorm +=

            cartesian_waypoints_planner_B.t_c *

            cartesian_waypoints_planner_B.t_c;

        }



        cartesian_waypoints_planner_B.b_j_k++;

      }



      cartesian_waypoints_planner_B.colnorm =

        cartesian_waypoints_planner_B.scale_f * sqrt

        (cartesian_waypoints_planner_B.colnorm);

      cartesian_waypoints_planner_B.b_j_k = 4;

      while (cartesian_waypoints_planner_B.b_j_k + 1 <= 8) {

        if (V[cartesian_waypoints_planner_B.b_j_k].im == 0.0) {

          cartesian_waypoints_planner_B.scale_f =

            V[cartesian_waypoints_planner_B.b_j_k].re /

            cartesian_waypoints_planner_B.colnorm;

          cartesian_waypoints_planner_B.absxk = 0.0;

        } else if (V[cartesian_waypoints_planner_B.b_j_k].re == 0.0) {

          cartesian_waypoints_planner_B.scale_f = 0.0;

          cartesian_waypoints_planner_B.absxk =

            V[cartesian_waypoints_planner_B.b_j_k].im /

            cartesian_waypoints_planner_B.colnorm;

        } else {

          cartesian_waypoints_planner_B.scale_f =

            V[cartesian_waypoints_planner_B.b_j_k].re /

            cartesian_waypoints_planner_B.colnorm;

          cartesian_waypoints_planner_B.absxk =

            V[cartesian_waypoints_planner_B.b_j_k].im /

            cartesian_waypoints_planner_B.colnorm;

        }



        V[cartesian_waypoints_planner_B.b_j_k].re =

          cartesian_waypoints_planner_B.scale_f;

        V[cartesian_waypoints_planner_B.b_j_k].im =

          cartesian_waypoints_planner_B.absxk;

        cartesian_waypoints_planner_B.b_j_k++;

      }



      cartesian_waypoints_planner_B.colnorm = 0.0;

      cartesian_waypoints_planner_B.scale_f = 3.3121686421112381E-170;

      cartesian_waypoints_planner_B.b_j_k = 8;

      while (cartesian_waypoints_planner_B.b_j_k + 1 <= 12) {

        cartesian_waypoints_planner_B.absxk = fabs

          (V[cartesian_waypoints_planner_B.b_j_k].re);

        if (cartesian_waypoints_planner_B.absxk >

            cartesian_waypoints_planner_B.scale_f) {

          cartesian_waypoints_planner_B.t_c =

            cartesian_waypoints_planner_B.scale_f /

            cartesian_waypoints_planner_B.absxk;

          cartesian_waypoints_planner_B.colnorm =

            cartesian_waypoints_planner_B.colnorm *

            cartesian_waypoints_planner_B.t_c *

            cartesian_waypoints_planner_B.t_c + 1.0;

          cartesian_waypoints_planner_B.scale_f =

            cartesian_waypoints_planner_B.absxk;

        } else {

          cartesian_waypoints_planner_B.t_c =

            cartesian_waypoints_planner_B.absxk /

            cartesian_waypoints_planner_B.scale_f;

          cartesian_waypoints_planner_B.colnorm +=

            cartesian_waypoints_planner_B.t_c *

            cartesian_waypoints_planner_B.t_c;

        }



        cartesian_waypoints_planner_B.absxk = fabs

          (V[cartesian_waypoints_planner_B.b_j_k].im);

        if (cartesian_waypoints_planner_B.absxk >

            cartesian_waypoints_planner_B.scale_f) {

          cartesian_waypoints_planner_B.t_c =

            cartesian_waypoints_planner_B.scale_f /

            cartesian_waypoints_planner_B.absxk;

          cartesian_waypoints_planner_B.colnorm =

            cartesian_waypoints_planner_B.colnorm *

            cartesian_waypoints_planner_B.t_c *

            cartesian_waypoints_planner_B.t_c + 1.0;

          cartesian_waypoints_planner_B.scale_f =

            cartesian_waypoints_planner_B.absxk;

        } else {

          cartesian_waypoints_planner_B.t_c =

            cartesian_waypoints_planner_B.absxk /

            cartesian_waypoints_planner_B.scale_f;

          cartesian_waypoints_planner_B.colnorm +=

            cartesian_waypoints_planner_B.t_c *

            cartesian_waypoints_planner_B.t_c;

        }



        cartesian_waypoints_planner_B.b_j_k++;

      }



      cartesian_waypoints_planner_B.colnorm =

        cartesian_waypoints_planner_B.scale_f * sqrt

        (cartesian_waypoints_planner_B.colnorm);

      cartesian_waypoints_planner_B.b_j_k = 8;

      while (cartesian_waypoints_planner_B.b_j_k + 1 <= 12) {

        if (V[cartesian_waypoints_planner_B.b_j_k].im == 0.0) {

          cartesian_waypoints_planner_B.scale_f =

            V[cartesian_waypoints_planner_B.b_j_k].re /

            cartesian_waypoints_planner_B.colnorm;

          cartesian_waypoints_planner_B.absxk = 0.0;

        } else if (V[cartesian_waypoints_planner_B.b_j_k].re == 0.0) {

          cartesian_waypoints_planner_B.scale_f = 0.0;

          cartesian_waypoints_planner_B.absxk =

            V[cartesian_waypoints_planner_B.b_j_k].im /

            cartesian_waypoints_planner_B.colnorm;

        } else {

          cartesian_waypoints_planner_B.scale_f =

            V[cartesian_waypoints_planner_B.b_j_k].re /

            cartesian_waypoints_planner_B.colnorm;

          cartesian_waypoints_planner_B.absxk =

            V[cartesian_waypoints_planner_B.b_j_k].im /

            cartesian_waypoints_planner_B.colnorm;

        }



        V[cartesian_waypoints_planner_B.b_j_k].re =

          cartesian_waypoints_planner_B.scale_f;

        V[cartesian_waypoints_planner_B.b_j_k].im =

          cartesian_waypoints_planner_B.absxk;

        cartesian_waypoints_planner_B.b_j_k++;

      }



      cartesian_waypoints_planner_B.colnorm = 0.0;

      cartesian_waypoints_planner_B.scale_f = 3.3121686421112381E-170;

      cartesian_waypoints_planner_B.b_j_k = 12;

      while (cartesian_waypoints_planner_B.b_j_k + 1 <= 16) {

        cartesian_waypoints_planner_B.absxk = fabs

          (V[cartesian_waypoints_planner_B.b_j_k].re);

        if (cartesian_waypoints_planner_B.absxk >

            cartesian_waypoints_planner_B.scale_f) {

          cartesian_waypoints_planner_B.t_c =

            cartesian_waypoints_planner_B.scale_f /

            cartesian_waypoints_planner_B.absxk;

          cartesian_waypoints_planner_B.colnorm =

            cartesian_waypoints_planner_B.colnorm *

            cartesian_waypoints_planner_B.t_c *

            cartesian_waypoints_planner_B.t_c + 1.0;

          cartesian_waypoints_planner_B.scale_f =

            cartesian_waypoints_planner_B.absxk;

        } else {

          cartesian_waypoints_planner_B.t_c =

            cartesian_waypoints_planner_B.absxk /

            cartesian_waypoints_planner_B.scale_f;

          cartesian_waypoints_planner_B.colnorm +=

            cartesian_waypoints_planner_B.t_c *

            cartesian_waypoints_planner_B.t_c;

        }



        cartesian_waypoints_planner_B.absxk = fabs

          (V[cartesian_waypoints_planner_B.b_j_k].im);

        if (cartesian_waypoints_planner_B.absxk >

            cartesian_waypoints_planner_B.scale_f) {

          cartesian_waypoints_planner_B.t_c =

            cartesian_waypoints_planner_B.scale_f /

            cartesian_waypoints_planner_B.absxk;

          cartesian_waypoints_planner_B.colnorm =

            cartesian_waypoints_planner_B.colnorm *

            cartesian_waypoints_planner_B.t_c *

            cartesian_waypoints_planner_B.t_c + 1.0;

          cartesian_waypoints_planner_B.scale_f =

            cartesian_waypoints_planner_B.absxk;

        } else {

          cartesian_waypoints_planner_B.t_c =

            cartesian_waypoints_planner_B.absxk /

            cartesian_waypoints_planner_B.scale_f;

          cartesian_waypoints_planner_B.colnorm +=

            cartesian_waypoints_planner_B.t_c *

            cartesian_waypoints_planner_B.t_c;

        }



        cartesian_waypoints_planner_B.b_j_k++;

      }



      cartesian_waypoints_planner_B.colnorm =

        cartesian_waypoints_planner_B.scale_f * sqrt

        (cartesian_waypoints_planner_B.colnorm);

      cartesian_waypoints_planner_B.b_j_k = 12;

      while (cartesian_waypoints_planner_B.b_j_k + 1 <= 16) {

        if (V[cartesian_waypoints_planner_B.b_j_k].im == 0.0) {

          cartesian_waypoints_planner_B.scale_f =

            V[cartesian_waypoints_planner_B.b_j_k].re /

            cartesian_waypoints_planner_B.colnorm;

          cartesian_waypoints_planner_B.absxk = 0.0;

        } else if (V[cartesian_waypoints_planner_B.b_j_k].re == 0.0) {

          cartesian_waypoints_planner_B.scale_f = 0.0;

          cartesian_waypoints_planner_B.absxk =

            V[cartesian_waypoints_planner_B.b_j_k].im /

            cartesian_waypoints_planner_B.colnorm;

        } else {

          cartesian_waypoints_planner_B.scale_f =

            V[cartesian_waypoints_planner_B.b_j_k].re /

            cartesian_waypoints_planner_B.colnorm;

          cartesian_waypoints_planner_B.absxk =

            V[cartesian_waypoints_planner_B.b_j_k].im /

            cartesian_waypoints_planner_B.colnorm;

        }



        V[cartesian_waypoints_planner_B.b_j_k].re =

          cartesian_waypoints_planner_B.scale_f;

        V[cartesian_waypoints_planner_B.b_j_k].im =

          cartesian_waypoints_planner_B.absxk;

        cartesian_waypoints_planner_B.b_j_k++;

      }



      if (cartesian_waypoints_planner_B.beta1[0].im == 0.0) {

        if (D[0].im == 0.0) {

          cartesian_waypoints_planner_B.scale_f = D[0].re /

            cartesian_waypoints_planner_B.beta1[0].re;

          cartesian_waypoints_planner_B.absxk = 0.0;

        } else if (D[0].re == 0.0) {

          cartesian_waypoints_planner_B.scale_f = 0.0;

          cartesian_waypoints_planner_B.absxk = D[0].im /

            cartesian_waypoints_planner_B.beta1[0].re;

        } else {

          cartesian_waypoints_planner_B.scale_f = D[0].re /

            cartesian_waypoints_planner_B.beta1[0].re;

          cartesian_waypoints_planner_B.absxk = D[0].im /

            cartesian_waypoints_planner_B.beta1[0].re;

        }

      } else if (cartesian_waypoints_planner_B.beta1[0].re == 0.0) {

        if (D[0].re == 0.0) {

          cartesian_waypoints_planner_B.scale_f = D[0].im /

            cartesian_waypoints_planner_B.beta1[0].im;

          cartesian_waypoints_planner_B.absxk = 0.0;

        } else if (D[0].im == 0.0) {

          cartesian_waypoints_planner_B.scale_f = 0.0;

          cartesian_waypoints_planner_B.absxk = -(D[0].re /

            cartesian_waypoints_planner_B.beta1[0].im);

        } else {

          cartesian_waypoints_planner_B.scale_f = D[0].im /

            cartesian_waypoints_planner_B.beta1[0].im;

          cartesian_waypoints_planner_B.absxk = -(D[0].re /

            cartesian_waypoints_planner_B.beta1[0].im);

        }

      } else {

        cartesian_waypoints_planner_B.colnorm = fabs

          (cartesian_waypoints_planner_B.beta1[0].re);

        cartesian_waypoints_planner_B.scale_f = fabs

          (cartesian_waypoints_planner_B.beta1[0].im);

        if (cartesian_waypoints_planner_B.colnorm >

            cartesian_waypoints_planner_B.scale_f) {

          cartesian_waypoints_planner_B.colnorm =

            cartesian_waypoints_planner_B.beta1[0].im /

            cartesian_waypoints_planner_B.beta1[0].re;

          cartesian_waypoints_planner_B.absxk =

            cartesian_waypoints_planner_B.colnorm *

            cartesian_waypoints_planner_B.beta1[0].im +

            cartesian_waypoints_planner_B.beta1[0].re;

          cartesian_waypoints_planner_B.scale_f =

            (cartesian_waypoints_planner_B.colnorm * D[0].im + D[0].re) /

            cartesian_waypoints_planner_B.absxk;

          cartesian_waypoints_planner_B.absxk = (D[0].im -

            cartesian_waypoints_planner_B.colnorm * D[0].re) /

            cartesian_waypoints_planner_B.absxk;

        } else if (cartesian_waypoints_planner_B.scale_f ==

                   cartesian_waypoints_planner_B.colnorm) {

          cartesian_waypoints_planner_B.absxk =

            cartesian_waypoints_planner_B.beta1[0].re > 0.0 ? 0.5 : -0.5;

          cartesian_waypoints_planner_B.t_c =

            cartesian_waypoints_planner_B.beta1[0].im > 0.0 ? 0.5 : -0.5;

          cartesian_waypoints_planner_B.scale_f = (D[0].re *

            cartesian_waypoints_planner_B.absxk + D[0].im *

            cartesian_waypoints_planner_B.t_c) /

            cartesian_waypoints_planner_B.colnorm;

          cartesian_waypoints_planner_B.absxk = (D[0].im *

            cartesian_waypoints_planner_B.absxk - D[0].re *

            cartesian_waypoints_planner_B.t_c) /

            cartesian_waypoints_planner_B.colnorm;

        } else {

          cartesian_waypoints_planner_B.colnorm =

            cartesian_waypoints_planner_B.beta1[0].re /

            cartesian_waypoints_planner_B.beta1[0].im;

          cartesian_waypoints_planner_B.absxk =

            cartesian_waypoints_planner_B.colnorm *

            cartesian_waypoints_planner_B.beta1[0].re +

            cartesian_waypoints_planner_B.beta1[0].im;

          cartesian_waypoints_planner_B.scale_f =

            (cartesian_waypoints_planner_B.colnorm * D[0].re + D[0].im) /

            cartesian_waypoints_planner_B.absxk;

          cartesian_waypoints_planner_B.absxk =

            (cartesian_waypoints_planner_B.colnorm * D[0].im - D[0].re) /

            cartesian_waypoints_planner_B.absxk;

        }

      }



      D[0].re = cartesian_waypoints_planner_B.scale_f;

      D[0].im = cartesian_waypoints_planner_B.absxk;

      if (cartesian_waypoints_planner_B.beta1[1].im == 0.0) {

        if (D[1].im == 0.0) {

          cartesian_waypoints_planner_B.scale_f = D[1].re /

            cartesian_waypoints_planner_B.beta1[1].re;

          cartesian_waypoints_planner_B.absxk = 0.0;

        } else if (D[1].re == 0.0) {

          cartesian_waypoints_planner_B.scale_f = 0.0;

          cartesian_waypoints_planner_B.absxk = D[1].im /

            cartesian_waypoints_planner_B.beta1[1].re;

        } else {

          cartesian_waypoints_planner_B.scale_f = D[1].re /

            cartesian_waypoints_planner_B.beta1[1].re;

          cartesian_waypoints_planner_B.absxk = D[1].im /

            cartesian_waypoints_planner_B.beta1[1].re;

        }

      } else if (cartesian_waypoints_planner_B.beta1[1].re == 0.0) {

        if (D[1].re == 0.0) {

          cartesian_waypoints_planner_B.scale_f = D[1].im /

            cartesian_waypoints_planner_B.beta1[1].im;

          cartesian_waypoints_planner_B.absxk = 0.0;

        } else if (D[1].im == 0.0) {

          cartesian_waypoints_planner_B.scale_f = 0.0;

          cartesian_waypoints_planner_B.absxk = -(D[1].re /

            cartesian_waypoints_planner_B.beta1[1].im);

        } else {

          cartesian_waypoints_planner_B.scale_f = D[1].im /

            cartesian_waypoints_planner_B.beta1[1].im;

          cartesian_waypoints_planner_B.absxk = -(D[1].re /

            cartesian_waypoints_planner_B.beta1[1].im);

        }

      } else {

        cartesian_waypoints_planner_B.colnorm = fabs

          (cartesian_waypoints_planner_B.beta1[1].re);

        cartesian_waypoints_planner_B.scale_f = fabs

          (cartesian_waypoints_planner_B.beta1[1].im);

        if (cartesian_waypoints_planner_B.colnorm >

            cartesian_waypoints_planner_B.scale_f) {

          cartesian_waypoints_planner_B.colnorm =

            cartesian_waypoints_planner_B.beta1[1].im /

            cartesian_waypoints_planner_B.beta1[1].re;

          cartesian_waypoints_planner_B.absxk =

            cartesian_waypoints_planner_B.colnorm *

            cartesian_waypoints_planner_B.beta1[1].im +

            cartesian_waypoints_planner_B.beta1[1].re;

          cartesian_waypoints_planner_B.scale_f =

            (cartesian_waypoints_planner_B.colnorm * D[1].im + D[1].re) /

            cartesian_waypoints_planner_B.absxk;

          cartesian_waypoints_planner_B.absxk = (D[1].im -

            cartesian_waypoints_planner_B.colnorm * D[1].re) /

            cartesian_waypoints_planner_B.absxk;

        } else if (cartesian_waypoints_planner_B.scale_f ==

                   cartesian_waypoints_planner_B.colnorm) {

          cartesian_waypoints_planner_B.absxk =

            cartesian_waypoints_planner_B.beta1[1].re > 0.0 ? 0.5 : -0.5;

          cartesian_waypoints_planner_B.t_c =

            cartesian_waypoints_planner_B.beta1[1].im > 0.0 ? 0.5 : -0.5;

          cartesian_waypoints_planner_B.scale_f = (D[1].re *

            cartesian_waypoints_planner_B.absxk + D[1].im *

            cartesian_waypoints_planner_B.t_c) /

            cartesian_waypoints_planner_B.colnorm;

          cartesian_waypoints_planner_B.absxk = (D[1].im *

            cartesian_waypoints_planner_B.absxk - D[1].re *

            cartesian_waypoints_planner_B.t_c) /

            cartesian_waypoints_planner_B.colnorm;

        } else {

          cartesian_waypoints_planner_B.colnorm =

            cartesian_waypoints_planner_B.beta1[1].re /

            cartesian_waypoints_planner_B.beta1[1].im;

          cartesian_waypoints_planner_B.absxk =

            cartesian_waypoints_planner_B.colnorm *

            cartesian_waypoints_planner_B.beta1[1].re +

            cartesian_waypoints_planner_B.beta1[1].im;

          cartesian_waypoints_planner_B.scale_f =

            (cartesian_waypoints_planner_B.colnorm * D[1].re + D[1].im) /

            cartesian_waypoints_planner_B.absxk;

          cartesian_waypoints_planner_B.absxk =

            (cartesian_waypoints_planner_B.colnorm * D[1].im - D[1].re) /

            cartesian_waypoints_planner_B.absxk;

        }

      }



      D[1].re = cartesian_waypoints_planner_B.scale_f;

      D[1].im = cartesian_waypoints_planner_B.absxk;

      if (cartesian_waypoints_planner_B.beta1[2].im == 0.0) {

        if (D[2].im == 0.0) {

          cartesian_waypoints_planner_B.scale_f = D[2].re /

            cartesian_waypoints_planner_B.beta1[2].re;

          cartesian_waypoints_planner_B.absxk = 0.0;

        } else if (D[2].re == 0.0) {

          cartesian_waypoints_planner_B.scale_f = 0.0;

          cartesian_waypoints_planner_B.absxk = D[2].im /

            cartesian_waypoints_planner_B.beta1[2].re;

        } else {

          cartesian_waypoints_planner_B.scale_f = D[2].re /

            cartesian_waypoints_planner_B.beta1[2].re;

          cartesian_waypoints_planner_B.absxk = D[2].im /

            cartesian_waypoints_planner_B.beta1[2].re;

        }

      } else if (cartesian_waypoints_planner_B.beta1[2].re == 0.0) {

        if (D[2].re == 0.0) {

          cartesian_waypoints_planner_B.scale_f = D[2].im /

            cartesian_waypoints_planner_B.beta1[2].im;

          cartesian_waypoints_planner_B.absxk = 0.0;

        } else if (D[2].im == 0.0) {

          cartesian_waypoints_planner_B.scale_f = 0.0;

          cartesian_waypoints_planner_B.absxk = -(D[2].re /

            cartesian_waypoints_planner_B.beta1[2].im);

        } else {

          cartesian_waypoints_planner_B.scale_f = D[2].im /

            cartesian_waypoints_planner_B.beta1[2].im;

          cartesian_waypoints_planner_B.absxk = -(D[2].re /

            cartesian_waypoints_planner_B.beta1[2].im);

        }

      } else {

        cartesian_waypoints_planner_B.colnorm = fabs

          (cartesian_waypoints_planner_B.beta1[2].re);

        cartesian_waypoints_planner_B.scale_f = fabs

          (cartesian_waypoints_planner_B.beta1[2].im);

        if (cartesian_waypoints_planner_B.colnorm >

            cartesian_waypoints_planner_B.scale_f) {

          cartesian_waypoints_planner_B.colnorm =

            cartesian_waypoints_planner_B.beta1[2].im /

            cartesian_waypoints_planner_B.beta1[2].re;

          cartesian_waypoints_planner_B.absxk =

            cartesian_waypoints_planner_B.colnorm *

            cartesian_waypoints_planner_B.beta1[2].im +

            cartesian_waypoints_planner_B.beta1[2].re;

          cartesian_waypoints_planner_B.scale_f =

            (cartesian_waypoints_planner_B.colnorm * D[2].im + D[2].re) /

            cartesian_waypoints_planner_B.absxk;

          cartesian_waypoints_planner_B.absxk = (D[2].im -

            cartesian_waypoints_planner_B.colnorm * D[2].re) /

            cartesian_waypoints_planner_B.absxk;

        } else if (cartesian_waypoints_planner_B.scale_f ==

                   cartesian_waypoints_planner_B.colnorm) {

          cartesian_waypoints_planner_B.absxk =

            cartesian_waypoints_planner_B.beta1[2].re > 0.0 ? 0.5 : -0.5;

          cartesian_waypoints_planner_B.t_c =

            cartesian_waypoints_planner_B.beta1[2].im > 0.0 ? 0.5 : -0.5;

          cartesian_waypoints_planner_B.scale_f = (D[2].re *

            cartesian_waypoints_planner_B.absxk + D[2].im *

            cartesian_waypoints_planner_B.t_c) /

            cartesian_waypoints_planner_B.colnorm;

          cartesian_waypoints_planner_B.absxk = (D[2].im *

            cartesian_waypoints_planner_B.absxk - D[2].re *

            cartesian_waypoints_planner_B.t_c) /

            cartesian_waypoints_planner_B.colnorm;

        } else {

          cartesian_waypoints_planner_B.colnorm =

            cartesian_waypoints_planner_B.beta1[2].re /

            cartesian_waypoints_planner_B.beta1[2].im;

          cartesian_waypoints_planner_B.absxk =

            cartesian_waypoints_planner_B.colnorm *

            cartesian_waypoints_planner_B.beta1[2].re +

            cartesian_waypoints_planner_B.beta1[2].im;

          cartesian_waypoints_planner_B.scale_f =

            (cartesian_waypoints_planner_B.colnorm * D[2].re + D[2].im) /

            cartesian_waypoints_planner_B.absxk;

          cartesian_waypoints_planner_B.absxk =

            (cartesian_waypoints_planner_B.colnorm * D[2].im - D[2].re) /

            cartesian_waypoints_planner_B.absxk;

        }

      }



      D[2].re = cartesian_waypoints_planner_B.scale_f;

      D[2].im = cartesian_waypoints_planner_B.absxk;

      if (cartesian_waypoints_planner_B.beta1[3].im == 0.0) {

        if (D[3].im == 0.0) {

          cartesian_waypoints_planner_B.scale_f = D[3].re /

            cartesian_waypoints_planner_B.beta1[3].re;

          cartesian_waypoints_planner_B.absxk = 0.0;

        } else if (D[3].re == 0.0) {

          cartesian_waypoints_planner_B.scale_f = 0.0;

          cartesian_waypoints_planner_B.absxk = D[3].im /

            cartesian_waypoints_planner_B.beta1[3].re;

        } else {

          cartesian_waypoints_planner_B.scale_f = D[3].re /

            cartesian_waypoints_planner_B.beta1[3].re;

          cartesian_waypoints_planner_B.absxk = D[3].im /

            cartesian_waypoints_planner_B.beta1[3].re;

        }

      } else if (cartesian_waypoints_planner_B.beta1[3].re == 0.0) {

        if (D[3].re == 0.0) {

          cartesian_waypoints_planner_B.scale_f = D[3].im /

            cartesian_waypoints_planner_B.beta1[3].im;

          cartesian_waypoints_planner_B.absxk = 0.0;

        } else if (D[3].im == 0.0) {

          cartesian_waypoints_planner_B.scale_f = 0.0;

          cartesian_waypoints_planner_B.absxk = -(D[3].re /

            cartesian_waypoints_planner_B.beta1[3].im);

        } else {

          cartesian_waypoints_planner_B.scale_f = D[3].im /

            cartesian_waypoints_planner_B.beta1[3].im;

          cartesian_waypoints_planner_B.absxk = -(D[3].re /

            cartesian_waypoints_planner_B.beta1[3].im);

        }

      } else {

        cartesian_waypoints_planner_B.colnorm = fabs

          (cartesian_waypoints_planner_B.beta1[3].re);

        cartesian_waypoints_planner_B.scale_f = fabs

          (cartesian_waypoints_planner_B.beta1[3].im);

        if (cartesian_waypoints_planner_B.colnorm >

            cartesian_waypoints_planner_B.scale_f) {

          cartesian_waypoints_planner_B.colnorm =

            cartesian_waypoints_planner_B.beta1[3].im /

            cartesian_waypoints_planner_B.beta1[3].re;

          cartesian_waypoints_planner_B.absxk =

            cartesian_waypoints_planner_B.colnorm *

            cartesian_waypoints_planner_B.beta1[3].im +

            cartesian_waypoints_planner_B.beta1[3].re;

          cartesian_waypoints_planner_B.scale_f =

            (cartesian_waypoints_planner_B.colnorm * D[3].im + D[3].re) /

            cartesian_waypoints_planner_B.absxk;

          cartesian_waypoints_planner_B.absxk = (D[3].im -

            cartesian_waypoints_planner_B.colnorm * D[3].re) /

            cartesian_waypoints_planner_B.absxk;

        } else if (cartesian_waypoints_planner_B.scale_f ==

                   cartesian_waypoints_planner_B.colnorm) {

          cartesian_waypoints_planner_B.absxk =

            cartesian_waypoints_planner_B.beta1[3].re > 0.0 ? 0.5 : -0.5;

          cartesian_waypoints_planner_B.t_c =

            cartesian_waypoints_planner_B.beta1[3].im > 0.0 ? 0.5 : -0.5;

          cartesian_waypoints_planner_B.scale_f = (D[3].re *

            cartesian_waypoints_planner_B.absxk + D[3].im *

            cartesian_waypoints_planner_B.t_c) /

            cartesian_waypoints_planner_B.colnorm;

          cartesian_waypoints_planner_B.absxk = (D[3].im *

            cartesian_waypoints_planner_B.absxk - D[3].re *

            cartesian_waypoints_planner_B.t_c) /

            cartesian_waypoints_planner_B.colnorm;

        } else {

          cartesian_waypoints_planner_B.colnorm =

            cartesian_waypoints_planner_B.beta1[3].re /

            cartesian_waypoints_planner_B.beta1[3].im;

          cartesian_waypoints_planner_B.absxk =

            cartesian_waypoints_planner_B.colnorm *

            cartesian_waypoints_planner_B.beta1[3].re +

            cartesian_waypoints_planner_B.beta1[3].im;

          cartesian_waypoints_planner_B.scale_f =

            (cartesian_waypoints_planner_B.colnorm * D[3].re + D[3].im) /

            cartesian_waypoints_planner_B.absxk;

          cartesian_waypoints_planner_B.absxk =

            (cartesian_waypoints_planner_B.colnorm * D[3].im - D[3].re) /

            cartesian_waypoints_planner_B.absxk;

        }

      }



      D[3].re = cartesian_waypoints_planner_B.scale_f;

      D[3].im = cartesian_waypoints_planner_B.absxk;

    }

  }

}



static void matlabCodegenHandle_matlabCodeg(ros_slros_internal_block_GetP_T *obj)

{

  if (!obj->matlabCodegenIsDeleted) {

    obj->matlabCodegenIsDeleted = true;

  }

}



static void matlabCodegenHandle_matlab_evqu(robotics_slmanip_internal_blo_T *obj)

{

  if (!obj->matlabCodegenIsDeleted) {

    obj->matlabCodegenIsDeleted = true;

  }

}



static void cartesian_wa_SystemCore_release(b_inverseKinematics_cartesian_T *obj)

{

  if (obj->isInitialized == 1) {

    obj->isInitialized = 2;

  }

}



static void cartesian_SystemCore_delete_evq(b_inverseKinematics_cartesian_T *obj)

{

  cartesian_wa_SystemCore_release(obj);

}



static void matlabCodegenHandle_matlabC_evq(b_inverseKinematics_cartesian_T *obj)

{

  if (!obj->matlabCodegenIsDeleted) {

    obj->matlabCodegenIsDeleted = true;

    cartesian_SystemCore_delete_evq(obj);

  }

}



static void emxFreeStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian_wa_T

  *pStruct)

{

  cartesian_waypoi_emxFree_char_T(&pStruct->Type);

  cartesian_waypoi_emxFree_real_T(&pStruct->MotionSubspace);

  cartesian_waypoi_emxFree_char_T(&pStruct->NameInternal);

  cartesian_waypoi_emxFree_real_T(&pStruct->PositionLimitsInternal);

  cartesian_waypoi_emxFree_real_T(&pStruct->HomePositionInternal);

}



static void emxFreeStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T

  *pStruct)

{

  cartesian_waypoi_emxFree_char_T(&pStruct->NameInternal);

  emxFreeStruct_c_rigidBodyJoint(&pStruct->JointInternal);

}



static void emxFreeStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T

  *pStruct)

{

  emxFreeStruct_v_robotics_manip_(&pStruct->Base);

}



static void emxFreeStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T

  *pStruct)

{

  cartesian_waypoi_emxFree_real_T(&pStruct->Limits);

}



static void emxFreeStruct_robotics_slmanip_(robotics_slmanip_internal_blo_T

  *pStruct)

{

  emxFreeStruct_y_robotics_manip_(&pStruct->TreeInternal);

  emxFreeStruct_b_inverseKinemati(&pStruct->IKInternal);

}



static void emxFreeStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T

  *pStruct)

{

  cartesian_waypoi_emxFree_char_T(&pStruct->NameInternal);

}



static void emxFreeStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T

  *pStruct)

{

  emxFreeStruct_w_robotics_manip_(&pStruct->Base);

}



static void emxFreeStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T

  *pStruct)

{

  cartesian_waypoi_emxFree_char_T(&pStruct->BodyName);

  cartesian_waypoi_emxFree_real_T(&pStruct->ErrTemp);

  cartesian_waypoi_emxFree_real_T(&pStruct->GradTemp);

}



static void emxFreeStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T

  *pStruct)

{

  cartesian_waypoi_emxFree_real_T(&pStruct->ConstraintMatrix);

  cartesian_waypoi_emxFree_real_T(&pStruct->ConstraintBound);

}



static void matlabCodegenHandle_mat_evqusng(ros_slros_internal_block_Subs_T *obj)

{

  if (!obj->matlabCodegenIsDeleted) {

    obj->matlabCodegenIsDeleted = true;

  }

}



static void emxFreeStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian__e_T

  *pStruct)

{

  cartesian_waypoi_emxFree_char_T(&pStruct->Type);

}



static void emxFreeStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T

  *pStruct)

{

  cartesian_waypoi_emxFree_char_T(&pStruct->NameInternal);

  emxFreeStruct_c_rigidBodyJoint1(&pStruct->JointInternal);

}



static void emxFreeStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T

  *pStruct)

{

  emxFreeStruct_o_robotics_manip_(&pStruct->Base);

}



static void emxFreeStruct_robotics_slmani_e(robotics_slmanip_internal_b_e_T

  *pStruct)

{

  emxFreeStruct_p_robotics_manip_(&pStruct->TreeInternal);

}



static void emxFreeStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T

  *pStruct)

{

  cartesian_waypoi_emxFree_char_T(&pStruct->NameInternal);

  emxFreeStruct_c_rigidBodyJoint1(&pStruct->JointInternal);

}



static void cartesian_wa_matlabCodegenHa_lb(ros_slros_internal_block_SetP_T *obj)

{

  if (!obj->matlabCodegenIsDeleted) {

    obj->matlabCodegenIsDeleted = true;

  }

}



static void matlabCodegenHandle_matla_evqus(ros_slros_internal_block_Publ_T *obj)

{

  if (!obj->matlabCodegenIsDeleted) {

    obj->matlabCodegenIsDeleted = true;

  }

}



static void emxInitStruct_c_rigidBodyJoint(c_rigidBodyJoint_cartesian_wa_T

  *pStruct)

{

  cartesian_waypoi_emxInit_char_T(&pStruct->Type, 2);

  cartesian_waypoi_emxInit_real_T(&pStruct->MotionSubspace, 2);

  cartesian_waypoi_emxInit_char_T(&pStruct->NameInternal, 2);

  cartesian_waypoi_emxInit_real_T(&pStruct->PositionLimitsInternal, 2);

  cartesian_waypoi_emxInit_real_T(&pStruct->HomePositionInternal, 1);

}



static void emxInitStruct_v_robotics_manip_(v_robotics_manip_internal_Rig_T

  *pStruct)

{

  cartesian_waypoi_emxInit_char_T(&pStruct->NameInternal, 2);

  emxInitStruct_c_rigidBodyJoint(&pStruct->JointInternal);

}



static void emxInitStruct_y_robotics_manip_(y_robotics_manip_internal_Rig_T

  *pStruct)

{

  emxInitStruct_v_robotics_manip_(&pStruct->Base);

}



static void emxInitStruct_b_inverseKinemati(b_inverseKinematics_cartesian_T

  *pStruct)

{

  cartesian_waypoi_emxInit_real_T(&pStruct->Limits, 2);

}



static void emxInitStruct_robotics_slmanip_(robotics_slmanip_internal_blo_T

  *pStruct)

{

  emxInitStruct_y_robotics_manip_(&pStruct->TreeInternal);

  emxInitStruct_b_inverseKinemati(&pStruct->IKInternal);

}



static void emxInitStruct_w_robotics_manip_(w_robotics_manip_internal_Rig_T

  *pStruct)

{

  cartesian_waypoi_emxInit_char_T(&pStruct->NameInternal, 2);

}



static void emxInitStruct_x_robotics_manip_(x_robotics_manip_internal_Rig_T

  *pStruct)

{

  emxInitStruct_w_robotics_manip_(&pStruct->Base);

}



static void emxInitStruct_f_robotics_manip_(f_robotics_manip_internal_IKE_T

  *pStruct)

{

  cartesian_waypoi_emxInit_char_T(&pStruct->BodyName, 2);

  cartesian_waypoi_emxInit_real_T(&pStruct->ErrTemp, 1);

  cartesian_waypoi_emxInit_real_T(&pStruct->GradTemp, 1);

}



static void emxInitStruct_h_robotics_core_i(h_robotics_core_internal_Damp_T

  *pStruct)

{

  cartesian_waypoi_emxInit_real_T(&pStruct->ConstraintMatrix, 2);

  cartesian_waypoi_emxInit_real_T(&pStruct->ConstraintBound, 1);

}



static void cartesia_twister_state_vector_e(uint32_T mt[625])

{

  uint32_T r;

  int32_T b_mti;

  r = 5489U;

  mt[0] = 5489U;

  for (b_mti = 0; b_mti < 623; b_mti++) {

    r = ((r >> 30U ^ r) * 1812433253U + b_mti) + 1U;

    mt[b_mti + 1] = r;

  }



  mt[624] = 624U;

}



static void cartesian_wa_eml_rand_mt19937ar(uint32_T state[625])

{

  memset(&state[0], 0, 625U * sizeof(uint32_T));

  cartesia_twister_state_vector_e(state);

}



static v_robotics_manip_internal_Rig_T *cartesian_w_RigidBody_RigidBody

  (v_robotics_manip_internal_Rig_T *obj)

{

  v_robotics_manip_internal_Rig_T *b_obj;

  int8_T msubspace_data[36];

  real_T poslim_data[12];

  emxArray_char_T_cartesian_way_T *switch_expression;

  boolean_T b_bool;

  int32_T b_kstr;

  char_T b[8];

  char_T b_0[9];

  int32_T loop_ub;

  int8_T tmp[6];

  static const char_T tmp_0[13] = { 'e', 'd', 'o', '_', 'b', 'a', 's', 'e', '_',

    'l', 'i', 'n', 'k' };



  static const real_T tmp_1[9] = { 0.012583419040406959, -0.00021487638648447484,

    -0.00022605919127205462, -0.00021487638648447484, 0.00052369449451288713,

    -0.00011525315957400814, -0.00022605919127205462, -0.00011525315957400814,

    0.012646079447789898 };



  static const real_T tmp_2[36] = { 0.012583419040406959,

    -0.00021487638648447484, -0.00022605919127205462, 0.0, -0.00392971169381184,

    0.00047022930128152475, -0.00021487638648447484, 0.00052369449451288713,

    -0.00011525315957400814, 0.00392971169381184, 0.0, -0.00449464704691423,

    -0.00022605919127205462, -0.00011525315957400814, 0.012646079447789898,

    -0.00047022930128152475, 0.00449464704691423, 0.0, 0.0, 0.00392971169381184,

    -0.00047022930128152475, 0.0785942338762368, 0.0, 0.0, -0.00392971169381184,

    0.0, 0.00449464704691423, 0.0, 0.0785942338762368, 0.0,

    0.00047022930128152475, -0.00449464704691423, 0.0, 0.0, 0.0,

    0.0785942338762368 };



  static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,

    1 };



  static const char_T tmp_4[15] = { 'w', 'o', 'r', 'l', 'd', '_', 'e', 'd', 'o',

    '_', 'j', 'o', 'i', 'n', 't' };



  static const char_T tmp_5[5] = { 'f', 'i', 'x', 'e', 'd' };



  static const char_T tmp_6[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_7[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  static const real_T tmp_8[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };



  static const real_T tmp_9[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };



  int32_T exitg1;

  b_obj = obj;

  b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];

  obj->NameInternal->size[0] = 1;

  obj->NameInternal->size[1] = 13;

  cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 13; b_kstr++) {

    obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];

  }



  obj->ParentIndex = 0.0;

  obj->MassInternal = 0.0785942338762368;

  obj->CenterOfMassInternal[0] = 0.057188;

  obj->CenterOfMassInternal[1] = 0.005983;

  obj->CenterOfMassInternal[2] = 0.05;

  for (b_kstr = 0; b_kstr < 9; b_kstr++) {

    obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 36; b_kstr++) {

    obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];

  }



  obj->JointInternal.InTree = false;

  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];

  }



  b_kstr = obj->JointInternal.NameInternal->size[0] *

    obj->JointInternal.NameInternal->size[1];

  obj->JointInternal.NameInternal->size[0] = 1;

  obj->JointInternal.NameInternal->size[1] = 15;

  cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 15; b_kstr++) {

    obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];

  }



  b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];

  obj->JointInternal.Type->size[0] = 1;

  obj->JointInternal.Type->size[1] = 5;

  cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);

  for (b_kstr = 0; b_kstr < 5; b_kstr++) {

    obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = obj->JointInternal.Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]

    - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    b[b_kstr] = tmp_6[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_7[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (b_kstr) {

   case 0:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 1;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 0;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -3.1415926535897931;

    poslim_data[1] = 3.1415926535897931;

    obj->JointInternal.VelocityNumber = 1.0;

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   case 1:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 0;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 1;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -0.5;

    poslim_data[1] = 0.5;

    obj->JointInternal.VelocityNumber = 1.0;

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   default:

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = 0;

    }



    poslim_data[0] = 0.0;

    poslim_data[1] = 0.0;

    obj->JointInternal.VelocityNumber = 0.0;

    obj->JointInternal.PositionNumber = 0.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 0.0;

    break;

  }



  b_kstr = obj->JointInternal.MotionSubspace->size[0] *

    obj->JointInternal.MotionSubspace->size[1];

  obj->JointInternal.MotionSubspace->size[0] = 6;

  obj->JointInternal.MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];

  }



  b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *

    obj->JointInternal.PositionLimitsInternal->size[1];

  obj->JointInternal.PositionLimitsInternal->size[0] = 1;

  obj->JointInternal.PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,

    b_kstr);

  for (b_kstr = 0; b_kstr < 2; b_kstr++) {

    obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];

  }



  b_kstr = obj->JointInternal.HomePositionInternal->size[0];

  obj->JointInternal.HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,

    b_kstr);

  for (b_kstr = 0; b_kstr < 1; b_kstr++) {

    obj->JointInternal.HomePositionInternal->data[0] = 0.0;

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.JointToParentTransform[b_kstr] = tmp_8[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_9[b_kstr];

  }



  b_kstr = obj->JointInternal.MotionSubspace->size[0] *

    obj->JointInternal.MotionSubspace->size[1];

  obj->JointInternal.MotionSubspace->size[0] = 6;

  obj->JointInternal.MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    obj->JointInternal.MotionSubspace->data[b_kstr] = 0.0;

  }



  obj->JointInternal.InTree = true;

  b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *

    obj->JointInternal.PositionLimitsInternal->size[1];

  obj->JointInternal.PositionLimitsInternal->size[0] = 1;

  obj->JointInternal.PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,

    b_kstr);

  obj->JointInternal.PositionLimitsInternal->data[0] = 0.0;

  obj->JointInternal.PositionLimitsInternal->data

    [obj->JointInternal.PositionLimitsInternal->size[0]] = 0.0;

  obj->JointInternal.JointAxisInternal[0] = 0.0;

  obj->JointInternal.JointAxisInternal[1] = 0.0;

  obj->JointInternal.JointAxisInternal[2] = 0.0;

  b_kstr = obj->JointInternal.HomePositionInternal->size[0];

  obj->JointInternal.HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,

    b_kstr);

  obj->JointInternal.HomePositionInternal->data[0] = 0.0;

  return b_obj;

}



static v_robotics_manip_internal_Rig_T *cartesian_RigidBody_RigidBody_e

  (v_robotics_manip_internal_Rig_T *obj)

{

  v_robotics_manip_internal_Rig_T *b_obj;

  int8_T msubspace_data[36];

  real_T poslim_data[12];

  emxArray_char_T_cartesian_way_T *switch_expression;

  boolean_T b_bool;

  int32_T b_kstr;

  char_T b[8];

  char_T b_0[9];

  int32_T loop_ub;

  int8_T tmp[6];

  static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',

    '1' };



  static const real_T tmp_1[9] = { 0.012559660892485551, 0.00032713982710414,

    -1.32683892634271E-6, 0.00032713982710414, 0.00018048007331848145,

    9.17416945099368E-5, -1.32683892634271E-6, 9.17416945099368E-5,

    0.012672078048055372 };



  static const real_T tmp_2[36] = { 0.012559660892485551, 0.00032713982710414,

    -1.32683892634271E-6, 0.0, -0.0, 0.0037143634929909515, 0.00032713982710414,

    0.00018048007331848145, 9.17416945099368E-5, 0.0, 0.0, 0.0029444543779393356,

    -1.32683892634271E-6, 9.17416945099368E-5, 0.012672078048055372,

    -0.0037143634929909515, -0.0029444543779393356, 0.0, 0.0, 0.0,

    -0.0037143634929909515, 0.0785942338762368, 0.0, 0.0, -0.0, 0.0,

    -0.0029444543779393356, 0.0, 0.0785942338762368, 0.0, 0.0037143634929909515,

    0.0029444543779393356, 0.0, 0.0, 0.0, 0.0785942338762368 };



  static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,

    1 };



  static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',

    '_', '1' };



  static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.337, 1.0 };



  static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };



  static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,

    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,

    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };



  int32_T exitg1;

  b_obj = obj;

  b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];

  obj->NameInternal->size[0] = 1;

  obj->NameInternal->size[1] = 10;

  cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 10; b_kstr++) {

    obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];

  }



  obj->ParentIndex = 1.0;

  obj->MassInternal = 0.0785942338762368;

  obj->CenterOfMassInternal[0] = -0.037464;

  obj->CenterOfMassInternal[1] = 0.04726;

  obj->CenterOfMassInternal[2] = 0.0;

  for (b_kstr = 0; b_kstr < 9; b_kstr++) {

    obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 36; b_kstr++) {

    obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];

  }



  obj->JointInternal.InTree = false;

  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];

  }



  b_kstr = obj->JointInternal.NameInternal->size[0] *

    obj->JointInternal.NameInternal->size[1];

  obj->JointInternal.NameInternal->size[0] = 1;

  obj->JointInternal.NameInternal->size[1] = 11;

  cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 11; b_kstr++) {

    obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];

  }



  b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];

  obj->JointInternal.Type->size[0] = 1;

  obj->JointInternal.Type->size[1] = 8;

  cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);

  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = obj->JointInternal.Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]

    - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    b[b_kstr] = tmp_5[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_6[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (b_kstr) {

   case 0:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 1;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 0;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -3.1415926535897931;

    poslim_data[1] = 3.1415926535897931;

    obj->JointInternal.VelocityNumber = 1.0;

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   case 1:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 0;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 1;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -0.5;

    poslim_data[1] = 0.5;

    obj->JointInternal.VelocityNumber = 1.0;

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   default:

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = 0;

    }



    poslim_data[0] = 0.0;

    poslim_data[1] = 0.0;

    obj->JointInternal.VelocityNumber = 0.0;

    obj->JointInternal.PositionNumber = 0.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 0.0;

    break;

  }



  b_kstr = obj->JointInternal.MotionSubspace->size[0] *

    obj->JointInternal.MotionSubspace->size[1];

  obj->JointInternal.MotionSubspace->size[0] = 6;

  obj->JointInternal.MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];

  }



  b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *

    obj->JointInternal.PositionLimitsInternal->size[1];

  obj->JointInternal.PositionLimitsInternal->size[0] = 1;

  obj->JointInternal.PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,

    b_kstr);

  for (b_kstr = 0; b_kstr < 2; b_kstr++) {

    obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];

  }



  b_kstr = obj->JointInternal.HomePositionInternal->size[0];

  obj->JointInternal.HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,

    b_kstr);

  for (b_kstr = 0; b_kstr < 1; b_kstr++) {

    obj->JointInternal.HomePositionInternal->data[0] = 0.0;

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];

  }



  b_kstr = obj->JointInternal.MotionSubspace->size[0] *

    obj->JointInternal.MotionSubspace->size[1];

  obj->JointInternal.MotionSubspace->size[0] = 6;

  obj->JointInternal.MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];

  }



  obj->JointInternal.InTree = true;

  b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *

    obj->JointInternal.PositionLimitsInternal->size[1];

  obj->JointInternal.PositionLimitsInternal->size[0] = 1;

  obj->JointInternal.PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,

    b_kstr);

  obj->JointInternal.PositionLimitsInternal->data[0] = -3.10668606855;

  obj->JointInternal.PositionLimitsInternal->data

    [obj->JointInternal.PositionLimitsInternal->size[0]] = 3.10668606855;

  obj->JointInternal.JointAxisInternal[0] = 0.0;

  obj->JointInternal.JointAxisInternal[1] = 0.0;

  obj->JointInternal.JointAxisInternal[2] = 1.0;

  b_kstr = obj->JointInternal.HomePositionInternal->size[0];

  obj->JointInternal.HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,

    b_kstr);

  obj->JointInternal.HomePositionInternal->data[0] = 0.0;

  return b_obj;

}



static v_robotics_manip_internal_Rig_T *cartesia_RigidBody_RigidBody_ev

  (v_robotics_manip_internal_Rig_T *obj)

{

  v_robotics_manip_internal_Rig_T *b_obj;

  int8_T msubspace_data[36];

  real_T poslim_data[12];

  emxArray_char_T_cartesian_way_T *switch_expression;

  boolean_T b_bool;

  int32_T b_kstr;

  char_T b[8];

  char_T b_0[9];

  int32_T loop_ub;

  int8_T tmp[6];

  static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',

    '2' };



  static const real_T tmp_1[9] = { 0.012452446533447339, 0.00097164648860403489,

    -0.0012079400901817208, 0.00097164648860403489, 0.0077631932790618,

    0.0060529024261622953, -0.0012079400901817208, 0.0060529024261622953,

    0.0051581160550583753 };



  static const real_T tmp_2[36] = { 0.012452446533447339, 0.00097164648860403489,

    -0.0012079400901817208, 0.0, 0.0036112478581453288, 0.0024995324199659588,

    0.00097164648860403489, 0.0077631932790618, 0.0060529024261622953,

    -0.0036112478581453288, 0.0, 0.0012907531029494371, -0.0012079400901817208,

    0.0060529024261622953, 0.0051581160550583753, -0.0024995324199659588,

    -0.0012907531029494371, 0.0, 0.0, -0.0036112478581453288,

    -0.0024995324199659588, 0.0785942338762368, 0.0, 0.0, 0.0036112478581453288,

    0.0, -0.0012907531029494371, 0.0, 0.0785942338762368, 0.0,

    0.0024995324199659588, 0.0012907531029494371, 0.0, 0.0, 0.0,

    0.0785942338762368 };



  static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,

    1 };



  static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',

    '_', '2' };



  static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,

    4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,

    0.0, 0.0, 0.0, 1.0 };



  static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };



  static const real_T tmp_9[36] = { 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,

    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,

    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };



  int32_T exitg1;

  b_obj = obj;

  b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];

  obj->NameInternal->size[0] = 1;

  obj->NameInternal->size[1] = 10;

  cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 10; b_kstr++) {

    obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];

  }



  obj->ParentIndex = 2.0;

  obj->MassInternal = 0.0785942338762368;

  obj->CenterOfMassInternal[0] = -0.016423;

  obj->CenterOfMassInternal[1] = 0.031803;

  obj->CenterOfMassInternal[2] = -0.045948;

  for (b_kstr = 0; b_kstr < 9; b_kstr++) {

    obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 36; b_kstr++) {

    obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];

  }



  obj->JointInternal.InTree = false;

  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];

  }



  b_kstr = obj->JointInternal.NameInternal->size[0] *

    obj->JointInternal.NameInternal->size[1];

  obj->JointInternal.NameInternal->size[0] = 1;

  obj->JointInternal.NameInternal->size[1] = 11;

  cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 11; b_kstr++) {

    obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];

  }



  b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];

  obj->JointInternal.Type->size[0] = 1;

  obj->JointInternal.Type->size[1] = 8;

  cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);

  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = obj->JointInternal.Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]

    - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    b[b_kstr] = tmp_5[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_6[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (b_kstr) {

   case 0:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 1;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 0;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -3.1415926535897931;

    poslim_data[1] = 3.1415926535897931;

    obj->JointInternal.VelocityNumber = 1.0;

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   case 1:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 0;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 1;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -0.5;

    poslim_data[1] = 0.5;

    obj->JointInternal.VelocityNumber = 1.0;

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   default:

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = 0;

    }



    poslim_data[0] = 0.0;

    poslim_data[1] = 0.0;

    obj->JointInternal.VelocityNumber = 0.0;

    obj->JointInternal.PositionNumber = 0.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 0.0;

    break;

  }



  b_kstr = obj->JointInternal.MotionSubspace->size[0] *

    obj->JointInternal.MotionSubspace->size[1];

  obj->JointInternal.MotionSubspace->size[0] = 6;

  obj->JointInternal.MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];

  }



  b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *

    obj->JointInternal.PositionLimitsInternal->size[1];

  obj->JointInternal.PositionLimitsInternal->size[0] = 1;

  obj->JointInternal.PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,

    b_kstr);

  for (b_kstr = 0; b_kstr < 2; b_kstr++) {

    obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];

  }



  b_kstr = obj->JointInternal.HomePositionInternal->size[0];

  obj->JointInternal.HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,

    b_kstr);

  for (b_kstr = 0; b_kstr < 1; b_kstr++) {

    obj->JointInternal.HomePositionInternal->data[0] = 0.0;

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];

  }



  b_kstr = obj->JointInternal.MotionSubspace->size[0] *

    obj->JointInternal.MotionSubspace->size[1];

  obj->JointInternal.MotionSubspace->size[0] = 6;

  obj->JointInternal.MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];

  }



  obj->JointInternal.InTree = true;

  b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *

    obj->JointInternal.PositionLimitsInternal->size[1];

  obj->JointInternal.PositionLimitsInternal->size[0] = 1;

  obj->JointInternal.PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,

    b_kstr);

  obj->JointInternal.PositionLimitsInternal->data[0] = -1.71042266695;

  obj->JointInternal.PositionLimitsInternal->data

    [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.71042266695;

  obj->JointInternal.JointAxisInternal[0] = 0.0;

  obj->JointInternal.JointAxisInternal[1] = 0.0;

  obj->JointInternal.JointAxisInternal[2] = -1.0;

  b_kstr = obj->JointInternal.HomePositionInternal->size[0];

  obj->JointInternal.HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,

    b_kstr);

  obj->JointInternal.HomePositionInternal->data[0] = 0.0;

  return b_obj;

}



static v_robotics_manip_internal_Rig_T *cartesi_RigidBody_RigidBody_evq

  (v_robotics_manip_internal_Rig_T *obj)

{

  v_robotics_manip_internal_Rig_T *b_obj;

  int8_T msubspace_data[36];

  real_T poslim_data[12];

  emxArray_char_T_cartesian_way_T *switch_expression;

  boolean_T b_bool;

  int32_T b_kstr;

  char_T b[8];

  char_T b_0[9];

  int32_T loop_ub;

  int8_T tmp[6];

  static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',

    '3' };



  static const real_T tmp_1[9] = { 0.0123976159829631, 0.00022039108420015264,

    2.1332825710116445E-6, 0.00022039108420015264, 0.00014803877895089843,

    -9.2077339045129963E-5, 2.1332825710116445E-6, -9.2077339045129963E-5,

    0.012477575138803739 };



  static const real_T tmp_2[36] = { 0.0123976159829631, 0.00022039108420015264,

    2.1332825710116445E-6, 0.0, -2.56217202436532E-5, 0.0010295844637787021,

    0.00022039108420015264, 0.00014803877895089843, -9.2077339045129963E-5,

    2.56217202436532E-5, 0.0, 0.0024737535112545539, 2.1332825710116445E-6,

    -9.2077339045129963E-5, 0.012477575138803739, -0.0010295844637787021,

    -0.0024737535112545539, 0.0, 0.0, 2.56217202436532E-5,

    -0.0010295844637787021, 0.0785942338762368, 0.0, 0.0, -2.56217202436532E-5,

    0.0, -0.0024737535112545539, 0.0, 0.0785942338762368, 0.0,

    0.0010295844637787021, 0.0024737535112545539, 0.0, 0.0, 0.0,

    0.0785942338762368 };



  static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,

    1 };



  static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',

    '_', '3' };



  static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  static const real_T tmp_7[16] = { 1.0, 2.0682310711021444E-13,

    2.0682310711021444E-13, 0.0, 2.0682310711021444E-13, -1.0, -0.0, 0.0,

    2.0682310711021444E-13, 4.2775797634723234E-26, -1.0, 0.0, 0.0, 0.2105, 0.0,

    1.0 };



  static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };



  static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,

    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,

    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };



  int32_T exitg1;

  b_obj = obj;

  b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];

  obj->NameInternal->size[0] = 1;

  obj->NameInternal->size[1] = 10;

  cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 10; b_kstr++) {

    obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];

  }



  obj->ParentIndex = 3.0;

  obj->MassInternal = 0.0785942338762368;

  obj->CenterOfMassInternal[0] = -0.031475;

  obj->CenterOfMassInternal[1] = 0.0131;

  obj->CenterOfMassInternal[2] = 0.000326;

  for (b_kstr = 0; b_kstr < 9; b_kstr++) {

    obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 36; b_kstr++) {

    obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];

  }



  obj->JointInternal.InTree = false;

  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];

  }



  b_kstr = obj->JointInternal.NameInternal->size[0] *

    obj->JointInternal.NameInternal->size[1];

  obj->JointInternal.NameInternal->size[0] = 1;

  obj->JointInternal.NameInternal->size[1] = 11;

  cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 11; b_kstr++) {

    obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];

  }



  b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];

  obj->JointInternal.Type->size[0] = 1;

  obj->JointInternal.Type->size[1] = 8;

  cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);

  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = obj->JointInternal.Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]

    - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    b[b_kstr] = tmp_5[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_6[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (b_kstr) {

   case 0:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 1;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 0;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -3.1415926535897931;

    poslim_data[1] = 3.1415926535897931;

    obj->JointInternal.VelocityNumber = 1.0;

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   case 1:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 0;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 1;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -0.5;

    poslim_data[1] = 0.5;

    obj->JointInternal.VelocityNumber = 1.0;

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   default:

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = 0;

    }



    poslim_data[0] = 0.0;

    poslim_data[1] = 0.0;

    obj->JointInternal.VelocityNumber = 0.0;

    obj->JointInternal.PositionNumber = 0.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 0.0;

    break;

  }



  b_kstr = obj->JointInternal.MotionSubspace->size[0] *

    obj->JointInternal.MotionSubspace->size[1];

  obj->JointInternal.MotionSubspace->size[0] = 6;

  obj->JointInternal.MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];

  }



  b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *

    obj->JointInternal.PositionLimitsInternal->size[1];

  obj->JointInternal.PositionLimitsInternal->size[0] = 1;

  obj->JointInternal.PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,

    b_kstr);

  for (b_kstr = 0; b_kstr < 2; b_kstr++) {

    obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];

  }



  b_kstr = obj->JointInternal.HomePositionInternal->size[0];

  obj->JointInternal.HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,

    b_kstr);

  for (b_kstr = 0; b_kstr < 1; b_kstr++) {

    obj->JointInternal.HomePositionInternal->data[0] = 0.0;

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];

  }



  b_kstr = obj->JointInternal.MotionSubspace->size[0] *

    obj->JointInternal.MotionSubspace->size[1];

  obj->JointInternal.MotionSubspace->size[0] = 6;

  obj->JointInternal.MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];

  }



  obj->JointInternal.InTree = true;

  b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *

    obj->JointInternal.PositionLimitsInternal->size[1];

  obj->JointInternal.PositionLimitsInternal->size[0] = 1;

  obj->JointInternal.PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,

    b_kstr);

  obj->JointInternal.PositionLimitsInternal->data[0] = -1.71042266695;

  obj->JointInternal.PositionLimitsInternal->data

    [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.71042266695;

  obj->JointInternal.JointAxisInternal[0] = 0.0;

  obj->JointInternal.JointAxisInternal[1] = 0.0;

  obj->JointInternal.JointAxisInternal[2] = 1.0;

  b_kstr = obj->JointInternal.HomePositionInternal->size[0];

  obj->JointInternal.HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,

    b_kstr);

  obj->JointInternal.HomePositionInternal->data[0] = 0.0;

  return b_obj;

}



static v_robotics_manip_internal_Rig_T *cartes_RigidBody_RigidBody_evqu

  (v_robotics_manip_internal_Rig_T *obj)

{

  v_robotics_manip_internal_Rig_T *b_obj;

  int8_T msubspace_data[36];

  real_T poslim_data[12];

  emxArray_char_T_cartesian_way_T *switch_expression;

  boolean_T b_bool;

  int32_T b_kstr;

  char_T b[8];

  char_T b_0[9];

  int32_T loop_ub;

  int8_T tmp[6];

  static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',

    '4' };



  static const real_T tmp_1[9] = { 0.0123990349928174, -2.7766271471639167E-6,

    0.00022466935228286869, -2.7766271471639167E-6, 0.012491487094789458,

    9.2330220708293281E-5, 0.00022466935228286869, 9.2330220708293281E-5,

    0.00016056153744711284 };



  static const real_T tmp_2[36] = { 0.0123990349928174, -2.7766271471639167E-6,

    0.00022466935228286869, 0.0, 0.0010818496293063995, 4.275526322867282E-5,

    -2.7766271471639167E-6, 0.012491487094789458, 9.2330220708293281E-5,

    -0.0010818496293063995, 0.0, -0.0026650518765093138, 0.00022466935228286869,

    9.2330220708293281E-5, 0.00016056153744711284, -4.275526322867282E-5,

    0.0026650518765093138, 0.0, 0.0, -0.0010818496293063995,

    -4.275526322867282E-5, 0.0785942338762368, 0.0, 0.0, 0.0010818496293063995,

    0.0, 0.0026650518765093138, 0.0, 0.0785942338762368, 0.0,

    4.275526322867282E-5, -0.0026650518765093138, 0.0, 0.0, 0.0,

    0.0785942338762368 };



  static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,

    1 };



  static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',

    '_', '4' };



  static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,

    4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,

    0.0, -0.268, 0.0, 1.0 };



  static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };



  static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,

    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,

    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };



  int32_T exitg1;

  b_obj = obj;

  b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];

  obj->NameInternal->size[0] = 1;

  obj->NameInternal->size[1] = 10;

  cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 10; b_kstr++) {

    obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];

  }



  obj->ParentIndex = 4.0;

  obj->MassInternal = 0.0785942338762368;

  obj->CenterOfMassInternal[0] = 0.033909;

  obj->CenterOfMassInternal[1] = 0.000544;

  obj->CenterOfMassInternal[2] = -0.013765;

  for (b_kstr = 0; b_kstr < 9; b_kstr++) {

    obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 36; b_kstr++) {

    obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];

  }



  obj->JointInternal.InTree = false;

  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];

  }



  b_kstr = obj->JointInternal.NameInternal->size[0] *

    obj->JointInternal.NameInternal->size[1];

  obj->JointInternal.NameInternal->size[0] = 1;

  obj->JointInternal.NameInternal->size[1] = 11;

  cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 11; b_kstr++) {

    obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];

  }



  b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];

  obj->JointInternal.Type->size[0] = 1;

  obj->JointInternal.Type->size[1] = 8;

  cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);

  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = obj->JointInternal.Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]

    - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    b[b_kstr] = tmp_5[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_6[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (b_kstr) {

   case 0:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 1;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 0;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -3.1415926535897931;

    poslim_data[1] = 3.1415926535897931;

    obj->JointInternal.VelocityNumber = 1.0;

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   case 1:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 0;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 1;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -0.5;

    poslim_data[1] = 0.5;

    obj->JointInternal.VelocityNumber = 1.0;

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   default:

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = 0;

    }



    poslim_data[0] = 0.0;

    poslim_data[1] = 0.0;

    obj->JointInternal.VelocityNumber = 0.0;

    obj->JointInternal.PositionNumber = 0.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 0.0;

    break;

  }



  b_kstr = obj->JointInternal.MotionSubspace->size[0] *

    obj->JointInternal.MotionSubspace->size[1];

  obj->JointInternal.MotionSubspace->size[0] = 6;

  obj->JointInternal.MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];

  }



  b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *

    obj->JointInternal.PositionLimitsInternal->size[1];

  obj->JointInternal.PositionLimitsInternal->size[0] = 1;

  obj->JointInternal.PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,

    b_kstr);

  for (b_kstr = 0; b_kstr < 2; b_kstr++) {

    obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];

  }



  b_kstr = obj->JointInternal.HomePositionInternal->size[0];

  obj->JointInternal.HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,

    b_kstr);

  for (b_kstr = 0; b_kstr < 1; b_kstr++) {

    obj->JointInternal.HomePositionInternal->data[0] = 0.0;

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];

  }



  b_kstr = obj->JointInternal.MotionSubspace->size[0] *

    obj->JointInternal.MotionSubspace->size[1];

  obj->JointInternal.MotionSubspace->size[0] = 6;

  obj->JointInternal.MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];

  }



  obj->JointInternal.InTree = true;

  b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *

    obj->JointInternal.PositionLimitsInternal->size[1];

  obj->JointInternal.PositionLimitsInternal->size[0] = 1;

  obj->JointInternal.PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,

    b_kstr);

  obj->JointInternal.PositionLimitsInternal->data[0] = -3.10668606855;

  obj->JointInternal.PositionLimitsInternal->data

    [obj->JointInternal.PositionLimitsInternal->size[0]] = 3.10668606855;

  obj->JointInternal.JointAxisInternal[0] = 0.0;

  obj->JointInternal.JointAxisInternal[1] = 0.0;

  obj->JointInternal.JointAxisInternal[2] = 1.0;

  b_kstr = obj->JointInternal.HomePositionInternal->size[0];

  obj->JointInternal.HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,

    b_kstr);

  obj->JointInternal.HomePositionInternal->data[0] = 0.0;

  return b_obj;

}



static v_robotics_manip_internal_Rig_T *carte_RigidBody_RigidBody_evqus

  (v_robotics_manip_internal_Rig_T *obj)

{

  v_robotics_manip_internal_Rig_T *b_obj;

  int8_T msubspace_data[36];

  real_T poslim_data[12];

  emxArray_char_T_cartesian_way_T *switch_expression;

  boolean_T b_bool;

  int32_T b_kstr;

  char_T b[8];

  char_T b_0[9];

  int32_T loop_ub;

  int8_T tmp[6];

  static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',

    '5' };



  static const real_T tmp_1[9] = { 0.012440329403329006, 2.388185677857016E-6,

    0.00012602126519218373, 2.388185677857016E-6, 0.012510746127660349,

    -9.077919321137075E-5, 0.00012602126519218373, -9.077919321137075E-5,

    0.00013851261456175015 };



  static const real_T tmp_2[36] = { 0.012440329403329006, 2.388185677857016E-6,

    0.00012602126519218373, 0.0, -0.0021015312196166957, -3.5996159115316455E-5,

    2.388185677857016E-6, 0.012510746127660349, -9.077919321137075E-5,

    0.0021015312196166957, 0.0, -0.0023173509858408423, 0.00012602126519218373,

    -9.077919321137075E-5, 0.00013851261456175015, 3.5996159115316455E-5,

    0.0023173509858408423, 0.0, 0.0, 0.0021015312196166957,

    3.5996159115316455E-5, 0.0785942338762368, 0.0, 0.0, -0.0021015312196166957,

    0.0, 0.0023173509858408423, 0.0, 0.0785942338762368, 0.0,

    -3.5996159115316455E-5, -0.0023173509858408423, 0.0, 0.0, 0.0,

    0.0785942338762368 };



  static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,

    1 };



  static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',

    '_', '5' };



  static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };



  static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };



  static const real_T tmp_9[36] = { 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,

    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,

    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };



  int32_T exitg1;

  b_obj = obj;

  b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];

  obj->NameInternal->size[0] = 1;

  obj->NameInternal->size[1] = 10;

  cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 10; b_kstr++) {

    obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];

  }



  obj->ParentIndex = 5.0;

  obj->MassInternal = 0.0785942338762368;

  obj->CenterOfMassInternal[0] = 0.029485;

  obj->CenterOfMassInternal[1] = -0.000458;

  obj->CenterOfMassInternal[2] = 0.026739;

  for (b_kstr = 0; b_kstr < 9; b_kstr++) {

    obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 36; b_kstr++) {

    obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];

  }



  obj->JointInternal.InTree = false;

  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];

  }



  b_kstr = obj->JointInternal.NameInternal->size[0] *

    obj->JointInternal.NameInternal->size[1];

  obj->JointInternal.NameInternal->size[0] = 1;

  obj->JointInternal.NameInternal->size[1] = 11;

  cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 11; b_kstr++) {

    obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];

  }



  b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];

  obj->JointInternal.Type->size[0] = 1;

  obj->JointInternal.Type->size[1] = 8;

  cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);

  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = obj->JointInternal.Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]

    - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    b[b_kstr] = tmp_5[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_6[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (b_kstr) {

   case 0:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 1;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 0;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -3.1415926535897931;

    poslim_data[1] = 3.1415926535897931;

    obj->JointInternal.VelocityNumber = 1.0;

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   case 1:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 0;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 1;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -0.5;

    poslim_data[1] = 0.5;

    obj->JointInternal.VelocityNumber = 1.0;

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   default:

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = 0;

    }



    poslim_data[0] = 0.0;

    poslim_data[1] = 0.0;

    obj->JointInternal.VelocityNumber = 0.0;

    obj->JointInternal.PositionNumber = 0.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 0.0;

    break;

  }



  b_kstr = obj->JointInternal.MotionSubspace->size[0] *

    obj->JointInternal.MotionSubspace->size[1];

  obj->JointInternal.MotionSubspace->size[0] = 6;

  obj->JointInternal.MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];

  }



  b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *

    obj->JointInternal.PositionLimitsInternal->size[1];

  obj->JointInternal.PositionLimitsInternal->size[0] = 1;

  obj->JointInternal.PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,

    b_kstr);

  for (b_kstr = 0; b_kstr < 2; b_kstr++) {

    obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];

  }



  b_kstr = obj->JointInternal.HomePositionInternal->size[0];

  obj->JointInternal.HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,

    b_kstr);

  for (b_kstr = 0; b_kstr < 1; b_kstr++) {

    obj->JointInternal.HomePositionInternal->data[0] = 0.0;

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];

  }



  b_kstr = obj->JointInternal.MotionSubspace->size[0] *

    obj->JointInternal.MotionSubspace->size[1];

  obj->JointInternal.MotionSubspace->size[0] = 6;

  obj->JointInternal.MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];

  }



  obj->JointInternal.InTree = true;

  b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *

    obj->JointInternal.PositionLimitsInternal->size[1];

  obj->JointInternal.PositionLimitsInternal->size[0] = 1;

  obj->JointInternal.PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,

    b_kstr);

  obj->JointInternal.PositionLimitsInternal->data[0] = -1.79768912955;

  obj->JointInternal.PositionLimitsInternal->data

    [obj->JointInternal.PositionLimitsInternal->size[0]] = 1.79768912955;

  obj->JointInternal.JointAxisInternal[0] = 0.0;

  obj->JointInternal.JointAxisInternal[1] = 1.0;

  obj->JointInternal.JointAxisInternal[2] = 0.0;

  b_kstr = obj->JointInternal.HomePositionInternal->size[0];

  obj->JointInternal.HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,

    b_kstr);

  obj->JointInternal.HomePositionInternal->data[0] = 0.0;

  return b_obj;

}



static v_robotics_manip_internal_Rig_T *cart_RigidBody_RigidBody_evqusn

  (v_robotics_manip_internal_Rig_T *obj)

{

  v_robotics_manip_internal_Rig_T *b_obj;

  int8_T msubspace_data[36];

  real_T poslim_data[12];

  emxArray_char_T_cartesian_way_T *switch_expression;

  boolean_T b_bool;

  int32_T b_kstr;

  char_T b[8];

  char_T b_0[9];

  int32_T loop_ub;

  int8_T tmp[6];

  static const char_T tmp_0[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',

    '6' };



  static const real_T tmp_1[9] = { 1.0067862401982823E-5, 2.0153545938371486E-9,

    5.3285284099072352E-9, 2.0153545938371486E-9, 1.4574493611914028E-5,

    1.6742291194075022E-9, 5.3285284099072352E-9, 1.6742291194075022E-9,

    1.006193323459457E-5 };



  static const real_T tmp_2[36] = { 1.0067862401982823E-5, 2.0153545938371486E-9,

    5.3285284099072352E-9, 0.0, -1.6782149839359722E-7, -0.00026087851925284686,

    2.0153545938371486E-9, 1.4574493611914028E-5, 1.6742291194075022E-9,

    1.6782149839359722E-7, 0.0, -1.957917481258634E-7, 5.3285284099072352E-9,

    1.6742291194075022E-9, 1.006193323459457E-5, 0.00026087851925284686,

    1.957917481258634E-7, 0.0, 0.0, 1.6782149839359722E-7,

    0.00026087851925284686, 0.0279702497322662, 0.0, 0.0, -1.6782149839359722E-7,

    0.0, 1.957917481258634E-7, 0.0, 0.0279702497322662, 0.0,

    -0.00026087851925284686, -1.957917481258634E-7, 0.0, 0.0, 0.0,

    0.0279702497322662 };



  static const int8_T tmp_3[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,

    1 };



  static const char_T tmp_4[11] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',

    '_', '6' };



  static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.1745, 1.0 };



  static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };



  static const real_T tmp_9[36] = { 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,

    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,

    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };



  int32_T exitg1;

  b_obj = obj;

  b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];

  obj->NameInternal->size[0] = 1;

  obj->NameInternal->size[1] = 10;

  cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 10; b_kstr++) {

    obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];

  }



  obj->ParentIndex = 6.0;

  obj->MassInternal = 0.0279702497322662;

  obj->CenterOfMassInternal[0] = 7.0E-6;

  obj->CenterOfMassInternal[1] = -0.009327;

  obj->CenterOfMassInternal[2] = 6.0E-6;

  for (b_kstr = 0; b_kstr < 9; b_kstr++) {

    obj->InertiaInternal[b_kstr] = tmp_1[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 36; b_kstr++) {

    obj->SpatialInertia[b_kstr] = tmp_2[b_kstr];

  }



  obj->JointInternal.InTree = false;

  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];

  }



  b_kstr = obj->JointInternal.NameInternal->size[0] *

    obj->JointInternal.NameInternal->size[1];

  obj->JointInternal.NameInternal->size[0] = 1;

  obj->JointInternal.NameInternal->size[1] = 11;

  cartes_emxEnsureCapacity_char_T(obj->JointInternal.NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 11; b_kstr++) {

    obj->JointInternal.NameInternal->data[b_kstr] = tmp_4[b_kstr];

  }



  b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];

  obj->JointInternal.Type->size[0] = 1;

  obj->JointInternal.Type->size[1] = 8;

  cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);

  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    obj->JointInternal.Type->data[b_kstr] = tmp_5[b_kstr];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = obj->JointInternal.Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]

    - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    b[b_kstr] = tmp_5[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_6[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (b_kstr) {

   case 0:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 1;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 0;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -3.1415926535897931;

    poslim_data[1] = 3.1415926535897931;

    obj->JointInternal.VelocityNumber = 1.0;

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   case 1:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 0;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 1;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -0.5;

    poslim_data[1] = 0.5;

    obj->JointInternal.VelocityNumber = 1.0;

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   default:

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = 0;

    }



    poslim_data[0] = 0.0;

    poslim_data[1] = 0.0;

    obj->JointInternal.VelocityNumber = 0.0;

    obj->JointInternal.PositionNumber = 0.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 0.0;

    break;

  }



  b_kstr = obj->JointInternal.MotionSubspace->size[0] *

    obj->JointInternal.MotionSubspace->size[1];

  obj->JointInternal.MotionSubspace->size[0] = 6;

  obj->JointInternal.MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    obj->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];

  }



  b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *

    obj->JointInternal.PositionLimitsInternal->size[1];

  obj->JointInternal.PositionLimitsInternal->size[0] = 1;

  obj->JointInternal.PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,

    b_kstr);

  for (b_kstr = 0; b_kstr < 2; b_kstr++) {

    obj->JointInternal.PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];

  }



  b_kstr = obj->JointInternal.HomePositionInternal->size[0];

  obj->JointInternal.HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,

    b_kstr);

  for (b_kstr = 0; b_kstr < 1; b_kstr++) {

    obj->JointInternal.HomePositionInternal->data[0] = 0.0;

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];

  }



  b_kstr = obj->JointInternal.MotionSubspace->size[0] *

    obj->JointInternal.MotionSubspace->size[1];

  obj->JointInternal.MotionSubspace->size[0] = 6;

  obj->JointInternal.MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    obj->JointInternal.MotionSubspace->data[b_kstr] = tmp_9[b_kstr];

  }



  obj->JointInternal.InTree = true;

  b_kstr = obj->JointInternal.PositionLimitsInternal->size[0] *

    obj->JointInternal.PositionLimitsInternal->size[1];

  obj->JointInternal.PositionLimitsInternal->size[0] = 1;

  obj->JointInternal.PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.PositionLimitsInternal,

    b_kstr);

  obj->JointInternal.PositionLimitsInternal->data[0] = -4.71238898038;

  obj->JointInternal.PositionLimitsInternal->data

    [obj->JointInternal.PositionLimitsInternal->size[0]] = 4.71238898038;

  obj->JointInternal.JointAxisInternal[0] = 0.0;

  obj->JointInternal.JointAxisInternal[1] = 0.0;

  obj->JointInternal.JointAxisInternal[2] = 1.0;

  b_kstr = obj->JointInternal.HomePositionInternal->size[0];

  obj->JointInternal.HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(obj->JointInternal.HomePositionInternal,

    b_kstr);

  obj->JointInternal.HomePositionInternal->data[0] = 0.0;

  return b_obj;

}



static y_robotics_manip_internal_Rig_T *car_RigidBodyTree_RigidBodyTree

  (y_robotics_manip_internal_Rig_T *obj, v_robotics_manip_internal_Rig_T *iobj_0,

   v_robotics_manip_internal_Rig_T *iobj_1, v_robotics_manip_internal_Rig_T

   *iobj_2, v_robotics_manip_internal_Rig_T *iobj_3,

   v_robotics_manip_internal_Rig_T *iobj_4, v_robotics_manip_internal_Rig_T

   *iobj_5, v_robotics_manip_internal_Rig_T *iobj_6,

   v_robotics_manip_internal_Rig_T *iobj_7)

{

  y_robotics_manip_internal_Rig_T *b_obj;

  v_robotics_manip_internal_Rig_T *obj_0;

  int8_T msubspace_data[36];

  real_T poslim_data[12];

  int8_T b_I[9];

  emxArray_char_T_cartesian_way_T *switch_expression;

  boolean_T b_bool;

  int32_T b_kstr;

  char_T b[8];

  char_T b_0[9];

  int32_T loop_ub;

  int8_T tmp[6];

  static const char_T tmp_0[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',

    'e', 'e' };



  static const real_T tmp_1[36] = { 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0,

    0.0, 0.0, -0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0,

    -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0 };



  static const int8_T tmp_2[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,

    1 };



  static const char_T tmp_3[12] = { 'e', 'd', 'o', '_', 'j', 'o', 'i', 'n', 't',

    '_', 'e', 'e' };



  static const char_T tmp_4[5] = { 'f', 'i', 'x', 'e', 'd' };



  static const char_T tmp_5[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_6[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  static const real_T tmp_7[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };



  static const real_T tmp_8[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };



  static const char_T tmp_9[5] = { 'w', 'o', 'r', 'l', 'd' };



  static const char_T tmp_a[9] = { 'w', 'o', 'r', 'l', 'd', '_', 'j', 'n', 't' };



  int32_T exitg1;

  b_obj = obj;

  obj->Bodies[0] = cartesian_w_RigidBody_RigidBody(iobj_7);

  obj->Bodies[0]->Index = 1.0;

  obj->Bodies[1] = cartesian_RigidBody_RigidBody_e(iobj_0);

  obj->Bodies[1]->Index = 2.0;

  obj->Bodies[2] = cartesia_RigidBody_RigidBody_ev(iobj_1);

  obj->Bodies[2]->Index = 3.0;

  obj->Bodies[3] = cartesi_RigidBody_RigidBody_evq(iobj_2);

  obj->Bodies[3]->Index = 4.0;

  obj->Bodies[4] = cartes_RigidBody_RigidBody_evqu(iobj_3);

  obj->Bodies[4]->Index = 5.0;

  obj->Bodies[5] = carte_RigidBody_RigidBody_evqus(iobj_4);

  obj->Bodies[5]->Index = 6.0;

  obj->Bodies[6] = cart_RigidBody_RigidBody_evqusn(iobj_5);

  obj->Bodies[6]->Index = 7.0;

  b_kstr = iobj_6->NameInternal->size[0] * iobj_6->NameInternal->size[1];

  iobj_6->NameInternal->size[0] = 1;

  iobj_6->NameInternal->size[1] = 11;

  cartes_emxEnsureCapacity_char_T(iobj_6->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 11; b_kstr++) {

    iobj_6->NameInternal->data[b_kstr] = tmp_0[b_kstr];

  }



  iobj_6->ParentIndex = 7.0;

  iobj_6->MassInternal = 0.0;

  iobj_6->CenterOfMassInternal[0] = 0.0;

  iobj_6->CenterOfMassInternal[1] = 0.0;

  iobj_6->CenterOfMassInternal[2] = 0.0;

  for (b_kstr = 0; b_kstr < 9; b_kstr++) {

    iobj_6->InertiaInternal[b_kstr] = 0.0;

  }



  for (b_kstr = 0; b_kstr < 36; b_kstr++) {

    iobj_6->SpatialInertia[b_kstr] = tmp_1[b_kstr];

  }



  iobj_6->JointInternal.InTree = false;

  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    iobj_6->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    iobj_6->JointInternal.ChildToJointTransform[b_kstr] = tmp_2[b_kstr];

  }



  b_kstr = iobj_6->JointInternal.NameInternal->size[0] *

    iobj_6->JointInternal.NameInternal->size[1];

  iobj_6->JointInternal.NameInternal->size[0] = 1;

  iobj_6->JointInternal.NameInternal->size[1] = 12;

  cartes_emxEnsureCapacity_char_T(iobj_6->JointInternal.NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 12; b_kstr++) {

    iobj_6->JointInternal.NameInternal->data[b_kstr] = tmp_3[b_kstr];

  }



  b_kstr = iobj_6->JointInternal.Type->size[0] * iobj_6->

    JointInternal.Type->size[1];

  iobj_6->JointInternal.Type->size[0] = 1;

  iobj_6->JointInternal.Type->size[1] = 5;

  cartes_emxEnsureCapacity_char_T(iobj_6->JointInternal.Type, b_kstr);

  for (b_kstr = 0; b_kstr < 5; b_kstr++) {

    iobj_6->JointInternal.Type->data[b_kstr] = tmp_4[b_kstr];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = iobj_6->JointInternal.Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = iobj_6->JointInternal.Type->size[0] * iobj_6->

    JointInternal.Type->size[1] - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = iobj_6->JointInternal.Type->data[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    b[b_kstr] = tmp_5[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_6[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  switch (b_kstr) {

   case 0:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 1;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 0;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -3.1415926535897931;

    poslim_data[1] = 3.1415926535897931;

    iobj_6->JointInternal.VelocityNumber = 1.0;

    iobj_6->JointInternal.PositionNumber = 1.0;

    iobj_6->JointInternal.JointAxisInternal[0] = 0.0;

    iobj_6->JointInternal.JointAxisInternal[1] = 0.0;

    iobj_6->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   case 1:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 0;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 1;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -0.5;

    poslim_data[1] = 0.5;

    iobj_6->JointInternal.VelocityNumber = 1.0;

    iobj_6->JointInternal.PositionNumber = 1.0;

    iobj_6->JointInternal.JointAxisInternal[0] = 0.0;

    iobj_6->JointInternal.JointAxisInternal[1] = 0.0;

    iobj_6->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   default:

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = 0;

    }



    poslim_data[0] = 0.0;

    poslim_data[1] = 0.0;

    iobj_6->JointInternal.VelocityNumber = 0.0;

    iobj_6->JointInternal.PositionNumber = 0.0;

    iobj_6->JointInternal.JointAxisInternal[0] = 0.0;

    iobj_6->JointInternal.JointAxisInternal[1] = 0.0;

    iobj_6->JointInternal.JointAxisInternal[2] = 0.0;

    break;

  }



  b_kstr = iobj_6->JointInternal.MotionSubspace->size[0] *

    iobj_6->JointInternal.MotionSubspace->size[1];

  iobj_6->JointInternal.MotionSubspace->size[0] = 6;

  iobj_6->JointInternal.MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    iobj_6->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];

  }



  b_kstr = iobj_6->JointInternal.PositionLimitsInternal->size[0] *

    iobj_6->JointInternal.PositionLimitsInternal->size[1];

  iobj_6->JointInternal.PositionLimitsInternal->size[0] = 1;

  iobj_6->JointInternal.PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.PositionLimitsInternal,

    b_kstr);

  for (b_kstr = 0; b_kstr < 2; b_kstr++) {

    iobj_6->JointInternal.PositionLimitsInternal->data[b_kstr] =

      poslim_data[b_kstr];

  }



  b_kstr = iobj_6->JointInternal.HomePositionInternal->size[0];

  iobj_6->JointInternal.HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.HomePositionInternal,

    b_kstr);

  for (b_kstr = 0; b_kstr < 1; b_kstr++) {

    iobj_6->JointInternal.HomePositionInternal->data[0] = 0.0;

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    iobj_6->JointInternal.JointToParentTransform[b_kstr] = tmp_7[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    iobj_6->JointInternal.ChildToJointTransform[b_kstr] = tmp_8[b_kstr];

  }



  b_kstr = iobj_6->JointInternal.MotionSubspace->size[0] *

    iobj_6->JointInternal.MotionSubspace->size[1];

  iobj_6->JointInternal.MotionSubspace->size[0] = 6;

  iobj_6->JointInternal.MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    iobj_6->JointInternal.MotionSubspace->data[b_kstr] = 0.0;

  }



  iobj_6->JointInternal.InTree = true;

  b_kstr = iobj_6->JointInternal.PositionLimitsInternal->size[0] *

    iobj_6->JointInternal.PositionLimitsInternal->size[1];

  iobj_6->JointInternal.PositionLimitsInternal->size[0] = 1;

  iobj_6->JointInternal.PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.PositionLimitsInternal,

    b_kstr);

  iobj_6->JointInternal.PositionLimitsInternal->data[0] = 0.0;

  iobj_6->JointInternal.PositionLimitsInternal->data

    [iobj_6->JointInternal.PositionLimitsInternal->size[0]] = 0.0;

  iobj_6->JointInternal.JointAxisInternal[0] = 0.0;

  iobj_6->JointInternal.JointAxisInternal[1] = 0.0;

  iobj_6->JointInternal.JointAxisInternal[2] = 0.0;

  b_kstr = iobj_6->JointInternal.HomePositionInternal->size[0];

  iobj_6->JointInternal.HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_6->JointInternal.HomePositionInternal,

    b_kstr);

  iobj_6->JointInternal.HomePositionInternal->data[0] = 0.0;

  obj->Bodies[7] = iobj_6;

  obj->Bodies[7]->Index = 8.0;

  obj->NumBodies = 8.0;

  obj->Gravity[0] = 0.0;

  obj->Gravity[1] = 0.0;

  obj->Gravity[2] = 0.0;

  obj_0 = &obj->Base;

  b_kstr = obj->Base.NameInternal->size[0] * obj->Base.NameInternal->size[1];

  obj->Base.NameInternal->size[0] = 1;

  obj->Base.NameInternal->size[1] = 5;

  cartes_emxEnsureCapacity_char_T(obj->Base.NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 5; b_kstr++) {

    obj->Base.NameInternal->data[b_kstr] = tmp_9[b_kstr];

  }



  obj->Base.JointInternal.InTree = false;

  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj_0->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj_0->JointInternal.ChildToJointTransform[b_kstr] = tmp_2[b_kstr];

  }



  b_kstr = obj->Base.JointInternal.NameInternal->size[0] *

    obj->Base.JointInternal.NameInternal->size[1];

  obj->Base.JointInternal.NameInternal->size[0] = 1;

  obj->Base.JointInternal.NameInternal->size[1] = 9;

  cartes_emxEnsureCapacity_char_T(obj->Base.JointInternal.NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 9; b_kstr++) {

    obj_0->JointInternal.NameInternal->data[b_kstr] = tmp_a[b_kstr];

  }



  b_kstr = obj->Base.JointInternal.Type->size[0] * obj->

    Base.JointInternal.Type->size[1];

  obj->Base.JointInternal.Type->size[0] = 1;

  obj->Base.JointInternal.Type->size[1] = 5;

  cartes_emxEnsureCapacity_char_T(obj->Base.JointInternal.Type, b_kstr);

  for (b_kstr = 0; b_kstr < 5; b_kstr++) {

    obj_0->JointInternal.Type->data[b_kstr] = tmp_4[b_kstr];

  }



  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = obj->Base.JointInternal.Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = obj->Base.JointInternal.Type->size[0] * obj->

    Base.JointInternal.Type->size[1] - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = obj_0->JointInternal.Type->data[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_6[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (b_kstr) {

   case 0:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 1;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 0;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -3.1415926535897931;

    poslim_data[1] = 3.1415926535897931;

    obj->Base.JointInternal.VelocityNumber = 1.0;

    obj->Base.JointInternal.PositionNumber = 1.0;

    obj->Base.JointInternal.JointAxisInternal[0] = 0.0;

    obj->Base.JointInternal.JointAxisInternal[1] = 0.0;

    obj->Base.JointInternal.JointAxisInternal[2] = 1.0;

    break;



   case 1:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 0;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 1;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -0.5;

    poslim_data[1] = 0.5;

    obj->Base.JointInternal.VelocityNumber = 1.0;

    obj->Base.JointInternal.PositionNumber = 1.0;

    obj->Base.JointInternal.JointAxisInternal[0] = 0.0;

    obj->Base.JointInternal.JointAxisInternal[1] = 0.0;

    obj->Base.JointInternal.JointAxisInternal[2] = 1.0;

    break;



   default:

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = 0;

    }



    poslim_data[0] = 0.0;

    poslim_data[1] = 0.0;

    obj->Base.JointInternal.VelocityNumber = 0.0;

    obj->Base.JointInternal.PositionNumber = 0.0;

    obj->Base.JointInternal.JointAxisInternal[0] = 0.0;

    obj->Base.JointInternal.JointAxisInternal[1] = 0.0;

    obj->Base.JointInternal.JointAxisInternal[2] = 0.0;

    break;

  }



  b_kstr = obj->Base.JointInternal.MotionSubspace->size[0] *

    obj->Base.JointInternal.MotionSubspace->size[1];

  obj->Base.JointInternal.MotionSubspace->size[0] = 6;

  obj->Base.JointInternal.MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    obj_0->JointInternal.MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];

  }



  b_kstr = obj->Base.JointInternal.PositionLimitsInternal->size[0] *

    obj->Base.JointInternal.PositionLimitsInternal->size[1];

  obj->Base.JointInternal.PositionLimitsInternal->size[0] = 1;

  obj->Base.JointInternal.PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.PositionLimitsInternal,

    b_kstr);

  for (b_kstr = 0; b_kstr < 2; b_kstr++) {

    obj_0->JointInternal.PositionLimitsInternal->data[b_kstr] =

      poslim_data[b_kstr];

  }



  b_kstr = obj->Base.JointInternal.HomePositionInternal->size[0];

  obj->Base.JointInternal.HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(obj->Base.JointInternal.HomePositionInternal,

    b_kstr);

  for (b_kstr = 0; b_kstr < 1; b_kstr++) {

    obj_0->JointInternal.HomePositionInternal->data[0] = 0.0;

  }



  obj->Base.Index = -1.0;

  obj->Base.ParentIndex = -1.0;

  obj->Base.MassInternal = 1.0;

  obj->Base.CenterOfMassInternal[0] = 0.0;

  obj->Base.CenterOfMassInternal[1] = 0.0;

  obj->Base.CenterOfMassInternal[2] = 0.0;

  for (b_kstr = 0; b_kstr < 9; b_kstr++) {

    b_I[b_kstr] = 0;

  }



  b_I[0] = 1;

  b_I[4] = 1;

  b_I[8] = 1;

  for (b_kstr = 0; b_kstr < 9; b_kstr++) {

    obj->Base.InertiaInternal[b_kstr] = b_I[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 36; b_kstr++) {

    msubspace_data[b_kstr] = 0;

  }



  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    msubspace_data[b_kstr + 6 * b_kstr] = 1;

  }



  for (b_kstr = 0; b_kstr < 36; b_kstr++) {

    obj->Base.SpatialInertia[b_kstr] = msubspace_data[b_kstr];

  }



  return b_obj;

}



static void cartesian_waypoints_planne_rand(real_T r[5])

{

  for (cartesian_waypoints_planner_B.b_k_j = 0;

       cartesian_waypoints_planner_B.b_k_j < 5;

       cartesian_waypoints_planner_B.b_k_j++) {

    memcpy(&cartesian_waypoints_planner_B.uv[0],

           &cartesian_waypoints_planner_DW.state_m[0], 625U * sizeof(uint32_T));

    cartesian__eml_rand_mt19937ar_e(cartesian_waypoints_planner_B.uv,

      cartesian_waypoints_planner_DW.state_m,

      &r[cartesian_waypoints_planner_B.b_k_j]);

  }

}



static w_robotics_manip_internal_Rig_T *car_RigidBody_RigidBody_evqusng

  (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0)

{

  w_robotics_manip_internal_Rig_T *b_obj;

  int8_T msubspace_data[36];

  real_T poslim_data[12];

  emxArray_char_T_cartesian_way_T *switch_expression;

  boolean_T b_bool;

  int32_T b_kstr;

  char_T b[8];

  char_T b_0[9];

  int32_T loop_ub;

  int8_T tmp[6];

  static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',

    '\x01' };



  static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,

    1 };



  static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',

    '\x01', '_', 'j', 'n', 't' };



  static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };



  static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  int32_T exitg1;

  b_obj = obj;

  b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];

  obj->NameInternal->size[0] = 1;

  obj->NameInternal->size[1] = 10;

  cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 10; b_kstr++) {

    obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];

  }



  iobj_0->InTree = false;

  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];

  }



  b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];

  iobj_0->NameInternal->size[0] = 1;

  iobj_0->NameInternal->size[1] = 14;

  cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 14; b_kstr++) {

    iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];

  }



  b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];

  iobj_0->Type->size[0] = 1;

  iobj_0->Type->size[1] = 5;

  cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);

  for (b_kstr = 0; b_kstr < 5; b_kstr++) {

    iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = iobj_0->Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    b[b_kstr] = tmp_4[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_5[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (b_kstr) {

   case 0:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 1;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 0;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -3.1415926535897931;

    poslim_data[1] = 3.1415926535897931;

    iobj_0->VelocityNumber = 1.0;

    iobj_0->PositionNumber = 1.0;

    iobj_0->JointAxisInternal[0] = 0.0;

    iobj_0->JointAxisInternal[1] = 0.0;

    iobj_0->JointAxisInternal[2] = 1.0;

    break;



   case 1:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 0;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 1;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -0.5;

    poslim_data[1] = 0.5;

    iobj_0->VelocityNumber = 1.0;

    iobj_0->PositionNumber = 1.0;

    iobj_0->JointAxisInternal[0] = 0.0;

    iobj_0->JointAxisInternal[1] = 0.0;

    iobj_0->JointAxisInternal[2] = 1.0;

    break;



   default:

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = 0;

    }



    poslim_data[0] = 0.0;

    poslim_data[1] = 0.0;

    iobj_0->VelocityNumber = 0.0;

    iobj_0->PositionNumber = 0.0;

    iobj_0->JointAxisInternal[0] = 0.0;

    iobj_0->JointAxisInternal[1] = 0.0;

    iobj_0->JointAxisInternal[2] = 0.0;

    break;

  }



  b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];

  iobj_0->MotionSubspace->size[0] = 6;

  iobj_0->MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];

  }



  b_kstr = iobj_0->PositionLimitsInternal->size[0] *

    iobj_0->PositionLimitsInternal->size[1];

  iobj_0->PositionLimitsInternal->size[0] = 1;

  iobj_0->PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 2; b_kstr++) {

    iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];

  }



  b_kstr = iobj_0->HomePositionInternal->size[0];

  iobj_0->HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 1; b_kstr++) {

    iobj_0->HomePositionInternal->data[0] = 0.0;

  }



  obj->JointInternal = iobj_0;

  obj->Index = -1.0;

  obj->ParentIndex = -1.0;

  return b_obj;

}



static w_robotics_manip_internal_Rig_T *ca_RigidBody_RigidBody_evqusngv

  (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0)

{

  w_robotics_manip_internal_Rig_T *b_obj;

  int8_T msubspace_data[36];

  real_T poslim_data[12];

  emxArray_char_T_cartesian_way_T *switch_expression;

  boolean_T b_bool;

  int32_T b_kstr;

  char_T b[8];

  char_T b_0[9];

  int32_T loop_ub;

  int8_T tmp[6];

  static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',

    '\x02' };



  static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,

    1 };



  static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',

    '\x02', '_', 'j', 'n', 't' };



  static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };



  static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  int32_T exitg1;

  b_obj = obj;

  b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];

  obj->NameInternal->size[0] = 1;

  obj->NameInternal->size[1] = 10;

  cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 10; b_kstr++) {

    obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];

  }



  iobj_0->InTree = false;

  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];

  }



  b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];

  iobj_0->NameInternal->size[0] = 1;

  iobj_0->NameInternal->size[1] = 14;

  cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 14; b_kstr++) {

    iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];

  }



  b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];

  iobj_0->Type->size[0] = 1;

  iobj_0->Type->size[1] = 5;

  cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);

  for (b_kstr = 0; b_kstr < 5; b_kstr++) {

    iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = iobj_0->Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    b[b_kstr] = tmp_4[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_5[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (b_kstr) {

   case 0:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 1;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 0;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -3.1415926535897931;

    poslim_data[1] = 3.1415926535897931;

    iobj_0->VelocityNumber = 1.0;

    iobj_0->PositionNumber = 1.0;

    iobj_0->JointAxisInternal[0] = 0.0;

    iobj_0->JointAxisInternal[1] = 0.0;

    iobj_0->JointAxisInternal[2] = 1.0;

    break;



   case 1:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 0;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 1;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -0.5;

    poslim_data[1] = 0.5;

    iobj_0->VelocityNumber = 1.0;

    iobj_0->PositionNumber = 1.0;

    iobj_0->JointAxisInternal[0] = 0.0;

    iobj_0->JointAxisInternal[1] = 0.0;

    iobj_0->JointAxisInternal[2] = 1.0;

    break;



   default:

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = 0;

    }



    poslim_data[0] = 0.0;

    poslim_data[1] = 0.0;

    iobj_0->VelocityNumber = 0.0;

    iobj_0->PositionNumber = 0.0;

    iobj_0->JointAxisInternal[0] = 0.0;

    iobj_0->JointAxisInternal[1] = 0.0;

    iobj_0->JointAxisInternal[2] = 0.0;

    break;

  }



  b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];

  iobj_0->MotionSubspace->size[0] = 6;

  iobj_0->MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];

  }



  b_kstr = iobj_0->PositionLimitsInternal->size[0] *

    iobj_0->PositionLimitsInternal->size[1];

  iobj_0->PositionLimitsInternal->size[0] = 1;

  iobj_0->PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 2; b_kstr++) {

    iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];

  }



  b_kstr = iobj_0->HomePositionInternal->size[0];

  iobj_0->HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 1; b_kstr++) {

    iobj_0->HomePositionInternal->data[0] = 0.0;

  }



  obj->JointInternal = iobj_0;

  obj->Index = -1.0;

  obj->ParentIndex = -1.0;

  return b_obj;

}



static w_robotics_manip_internal_Rig_T *c_RigidBody_RigidBody_evqusngvo

  (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0)

{

  w_robotics_manip_internal_Rig_T *b_obj;

  int8_T msubspace_data[36];

  real_T poslim_data[12];

  emxArray_char_T_cartesian_way_T *switch_expression;

  boolean_T b_bool;

  int32_T b_kstr;

  char_T b[8];

  char_T b_0[9];

  int32_T loop_ub;

  int8_T tmp[6];

  static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',

    '\x03' };



  static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,

    1 };



  static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',

    '\x03', '_', 'j', 'n', 't' };



  static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };



  static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  int32_T exitg1;

  b_obj = obj;

  b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];

  obj->NameInternal->size[0] = 1;

  obj->NameInternal->size[1] = 10;

  cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 10; b_kstr++) {

    obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];

  }



  iobj_0->InTree = false;

  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];

  }



  b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];

  iobj_0->NameInternal->size[0] = 1;

  iobj_0->NameInternal->size[1] = 14;

  cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 14; b_kstr++) {

    iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];

  }



  b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];

  iobj_0->Type->size[0] = 1;

  iobj_0->Type->size[1] = 5;

  cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);

  for (b_kstr = 0; b_kstr < 5; b_kstr++) {

    iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = iobj_0->Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    b[b_kstr] = tmp_4[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_5[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (b_kstr) {

   case 0:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 1;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 0;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -3.1415926535897931;

    poslim_data[1] = 3.1415926535897931;

    iobj_0->VelocityNumber = 1.0;

    iobj_0->PositionNumber = 1.0;

    iobj_0->JointAxisInternal[0] = 0.0;

    iobj_0->JointAxisInternal[1] = 0.0;

    iobj_0->JointAxisInternal[2] = 1.0;

    break;



   case 1:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 0;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 1;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -0.5;

    poslim_data[1] = 0.5;

    iobj_0->VelocityNumber = 1.0;

    iobj_0->PositionNumber = 1.0;

    iobj_0->JointAxisInternal[0] = 0.0;

    iobj_0->JointAxisInternal[1] = 0.0;

    iobj_0->JointAxisInternal[2] = 1.0;

    break;



   default:

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = 0;

    }



    poslim_data[0] = 0.0;

    poslim_data[1] = 0.0;

    iobj_0->VelocityNumber = 0.0;

    iobj_0->PositionNumber = 0.0;

    iobj_0->JointAxisInternal[0] = 0.0;

    iobj_0->JointAxisInternal[1] = 0.0;

    iobj_0->JointAxisInternal[2] = 0.0;

    break;

  }



  b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];

  iobj_0->MotionSubspace->size[0] = 6;

  iobj_0->MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];

  }



  b_kstr = iobj_0->PositionLimitsInternal->size[0] *

    iobj_0->PositionLimitsInternal->size[1];

  iobj_0->PositionLimitsInternal->size[0] = 1;

  iobj_0->PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 2; b_kstr++) {

    iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];

  }



  b_kstr = iobj_0->HomePositionInternal->size[0];

  iobj_0->HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 1; b_kstr++) {

    iobj_0->HomePositionInternal->data[0] = 0.0;

  }



  obj->JointInternal = iobj_0;

  obj->Index = -1.0;

  obj->ParentIndex = -1.0;

  return b_obj;

}



static w_robotics_manip_internal_Rig_T *RigidBody_RigidBody_evqusngvo0

  (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0)

{

  w_robotics_manip_internal_Rig_T *b_obj;

  int8_T msubspace_data[36];

  real_T poslim_data[12];

  emxArray_char_T_cartesian_way_T *switch_expression;

  boolean_T b_bool;

  int32_T b_kstr;

  char_T b[8];

  char_T b_0[9];

  int32_T loop_ub;

  int8_T tmp[6];

  static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',

    '\x04' };



  static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,

    1 };



  static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',

    '\x04', '_', 'j', 'n', 't' };



  static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };



  static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  int32_T exitg1;

  b_obj = obj;

  b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];

  obj->NameInternal->size[0] = 1;

  obj->NameInternal->size[1] = 10;

  cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 10; b_kstr++) {

    obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];

  }



  iobj_0->InTree = false;

  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];

  }



  b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];

  iobj_0->NameInternal->size[0] = 1;

  iobj_0->NameInternal->size[1] = 14;

  cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 14; b_kstr++) {

    iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];

  }



  b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];

  iobj_0->Type->size[0] = 1;

  iobj_0->Type->size[1] = 5;

  cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);

  for (b_kstr = 0; b_kstr < 5; b_kstr++) {

    iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = iobj_0->Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    b[b_kstr] = tmp_4[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_5[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (b_kstr) {

   case 0:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 1;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 0;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -3.1415926535897931;

    poslim_data[1] = 3.1415926535897931;

    iobj_0->VelocityNumber = 1.0;

    iobj_0->PositionNumber = 1.0;

    iobj_0->JointAxisInternal[0] = 0.0;

    iobj_0->JointAxisInternal[1] = 0.0;

    iobj_0->JointAxisInternal[2] = 1.0;

    break;



   case 1:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 0;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 1;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -0.5;

    poslim_data[1] = 0.5;

    iobj_0->VelocityNumber = 1.0;

    iobj_0->PositionNumber = 1.0;

    iobj_0->JointAxisInternal[0] = 0.0;

    iobj_0->JointAxisInternal[1] = 0.0;

    iobj_0->JointAxisInternal[2] = 1.0;

    break;



   default:

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = 0;

    }



    poslim_data[0] = 0.0;

    poslim_data[1] = 0.0;

    iobj_0->VelocityNumber = 0.0;

    iobj_0->PositionNumber = 0.0;

    iobj_0->JointAxisInternal[0] = 0.0;

    iobj_0->JointAxisInternal[1] = 0.0;

    iobj_0->JointAxisInternal[2] = 0.0;

    break;

  }



  b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];

  iobj_0->MotionSubspace->size[0] = 6;

  iobj_0->MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];

  }



  b_kstr = iobj_0->PositionLimitsInternal->size[0] *

    iobj_0->PositionLimitsInternal->size[1];

  iobj_0->PositionLimitsInternal->size[0] = 1;

  iobj_0->PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 2; b_kstr++) {

    iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];

  }



  b_kstr = iobj_0->HomePositionInternal->size[0];

  iobj_0->HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 1; b_kstr++) {

    iobj_0->HomePositionInternal->data[0] = 0.0;

  }



  obj->JointInternal = iobj_0;

  obj->Index = -1.0;

  obj->ParentIndex = -1.0;

  return b_obj;

}



static w_robotics_manip_internal_Rig_T *RigidBody_RigidBody_evqusngvo0a

  (w_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0)

{

  w_robotics_manip_internal_Rig_T *b_obj;

  int8_T msubspace_data[36];

  real_T poslim_data[12];

  emxArray_char_T_cartesian_way_T *switch_expression;

  boolean_T b_bool;

  int32_T b_kstr;

  char_T b[8];

  char_T b_0[9];

  int32_T loop_ub;

  int8_T tmp[6];

  static const char_T tmp_0[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',

    '\x05' };



  static const int8_T tmp_1[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,

    1 };



  static const char_T tmp_2[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',

    '\x05', '_', 'j', 'n', 't' };



  static const char_T tmp_3[5] = { 'f', 'i', 'x', 'e', 'd' };



  static const char_T tmp_4[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_5[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  int32_T exitg1;

  b_obj = obj;

  b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];

  obj->NameInternal->size[0] = 1;

  obj->NameInternal->size[1] = 10;

  cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 10; b_kstr++) {

    obj->NameInternal->data[b_kstr] = tmp_0[b_kstr];

  }



  iobj_0->InTree = false;

  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    iobj_0->JointToParentTransform[b_kstr] = tmp_1[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    iobj_0->ChildToJointTransform[b_kstr] = tmp_1[b_kstr];

  }



  b_kstr = iobj_0->NameInternal->size[0] * iobj_0->NameInternal->size[1];

  iobj_0->NameInternal->size[0] = 1;

  iobj_0->NameInternal->size[1] = 14;

  cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 14; b_kstr++) {

    iobj_0->NameInternal->data[b_kstr] = tmp_2[b_kstr];

  }



  b_kstr = iobj_0->Type->size[0] * iobj_0->Type->size[1];

  iobj_0->Type->size[0] = 1;

  iobj_0->Type->size[1] = 5;

  cartes_emxEnsureCapacity_char_T(iobj_0->Type, b_kstr);

  for (b_kstr = 0; b_kstr < 5; b_kstr++) {

    iobj_0->Type->data[b_kstr] = tmp_3[b_kstr];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = iobj_0->Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = iobj_0->Type->size[0] * iobj_0->Type->size[1] - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = iobj_0->Type->data[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    b[b_kstr] = tmp_4[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_5[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (b_kstr) {

   case 0:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 1;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 0;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -3.1415926535897931;

    poslim_data[1] = 3.1415926535897931;

    iobj_0->VelocityNumber = 1.0;

    iobj_0->PositionNumber = 1.0;

    iobj_0->JointAxisInternal[0] = 0.0;

    iobj_0->JointAxisInternal[1] = 0.0;

    iobj_0->JointAxisInternal[2] = 1.0;

    break;



   case 1:

    tmp[0] = 0;

    tmp[1] = 0;

    tmp[2] = 0;

    tmp[3] = 0;

    tmp[4] = 0;

    tmp[5] = 1;

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = tmp[b_kstr];

    }



    poslim_data[0] = -0.5;

    poslim_data[1] = 0.5;

    iobj_0->VelocityNumber = 1.0;

    iobj_0->PositionNumber = 1.0;

    iobj_0->JointAxisInternal[0] = 0.0;

    iobj_0->JointAxisInternal[1] = 0.0;

    iobj_0->JointAxisInternal[2] = 1.0;

    break;



   default:

    for (b_kstr = 0; b_kstr < 6; b_kstr++) {

      msubspace_data[b_kstr] = 0;

    }



    poslim_data[0] = 0.0;

    poslim_data[1] = 0.0;

    iobj_0->VelocityNumber = 0.0;

    iobj_0->PositionNumber = 0.0;

    iobj_0->JointAxisInternal[0] = 0.0;

    iobj_0->JointAxisInternal[1] = 0.0;

    iobj_0->JointAxisInternal[2] = 0.0;

    break;

  }



  b_kstr = iobj_0->MotionSubspace->size[0] * iobj_0->MotionSubspace->size[1];

  iobj_0->MotionSubspace->size[0] = 6;

  iobj_0->MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace, b_kstr);

  for (b_kstr = 0; b_kstr < 6; b_kstr++) {

    iobj_0->MotionSubspace->data[b_kstr] = msubspace_data[b_kstr];

  }



  b_kstr = iobj_0->PositionLimitsInternal->size[0] *

    iobj_0->PositionLimitsInternal->size[1];

  iobj_0->PositionLimitsInternal->size[0] = 1;

  iobj_0->PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 2; b_kstr++) {

    iobj_0->PositionLimitsInternal->data[b_kstr] = poslim_data[b_kstr];

  }



  b_kstr = iobj_0->HomePositionInternal->size[0];

  iobj_0->HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 1; b_kstr++) {

    iobj_0->HomePositionInternal->data[0] = 0.0;

  }



  obj->JointInternal = iobj_0;

  obj->Index = -1.0;

  obj->ParentIndex = -1.0;

  return b_obj;

}



static void ca_RigidBodyTree_clearAllBodies(x_robotics_manip_internal_Rig_T *obj,

  w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T

  *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,

  w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T

  *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,

  w_robotics_manip_internal_Rig_T *iobj_6, c_rigidBodyJoint_cartesian_wa_T

  *iobj_7, c_rigidBodyJoint_cartesian_wa_T *iobj_8,

  c_rigidBodyJoint_cartesian_wa_T *iobj_9, c_rigidBodyJoint_cartesian_wa_T

  *iobj_10, c_rigidBodyJoint_cartesian_wa_T *iobj_11,

  c_rigidBodyJoint_cartesian_wa_T *iobj_12, c_rigidBodyJoint_cartesian_wa_T

  *iobj_13, c_rigidBodyJoint_cartesian_wa_T *iobj_14,

  w_robotics_manip_internal_Rig_T *iobj_15)

{

  emxArray_char_T_cartesian_way_T *switch_expression;

  static const char_T tmp[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',

    '\x06' };



  static const int8_T tmp_0[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0,

    1 };



  static const char_T tmp_1[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',

    '\x06', '_', 'j', 'n', 't' };



  static const char_T tmp_2[5] = { 'f', 'i', 'x', 'e', 'd' };



  static const char_T tmp_3[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_4[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  static const char_T tmp_5[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',

    '\x07' };



  static const char_T tmp_6[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',

    '\x07', '_', 'j', 'n', 't' };



  static const char_T tmp_7[10] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',

    '\x08' };



  static const char_T tmp_8[14] = { 'd', 'u', 'm', 'm', 'y', 'b', 'o', 'd', 'y',

    '\x08', '_', 'j', 'n', 't' };



  int32_T exitg1;

  obj->Bodies[0] = car_RigidBody_RigidBody_evqusng(iobj_15, iobj_14);

  obj->Bodies[1] = ca_RigidBody_RigidBody_evqusngv(iobj_0, iobj_7);

  obj->Bodies[2] = c_RigidBody_RigidBody_evqusngvo(iobj_1, iobj_8);

  obj->Bodies[3] = RigidBody_RigidBody_evqusngvo0(iobj_2, iobj_9);

  obj->Bodies[4] = RigidBody_RigidBody_evqusngvo0a(iobj_3, iobj_10);

  cartesian_waypoints_planner_B.b_kstr_j = iobj_4->NameInternal->size[0] *

    iobj_4->NameInternal->size[1];

  iobj_4->NameInternal->size[0] = 1;

  iobj_4->NameInternal->size[1] = 10;

  cartes_emxEnsureCapacity_char_T(iobj_4->NameInternal,

    cartesian_waypoints_planner_B.b_kstr_j);

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 10;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_4->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_j] =

      tmp[cartesian_waypoints_planner_B.b_kstr_j];

  }



  iobj_11->InTree = false;

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 16;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_11->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_j] =

      tmp_0[cartesian_waypoints_planner_B.b_kstr_j];

  }



  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 16;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_11->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_j] =

      tmp_0[cartesian_waypoints_planner_B.b_kstr_j];

  }



  cartesian_waypoints_planner_B.b_kstr_j = iobj_11->NameInternal->size[0] *

    iobj_11->NameInternal->size[1];

  iobj_11->NameInternal->size[0] = 1;

  iobj_11->NameInternal->size[1] = 14;

  cartes_emxEnsureCapacity_char_T(iobj_11->NameInternal,

    cartesian_waypoints_planner_B.b_kstr_j);

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 14;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_11->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_j] =

      tmp_1[cartesian_waypoints_planner_B.b_kstr_j];

  }



  cartesian_waypoints_planner_B.b_kstr_j = iobj_11->Type->size[0] *

    iobj_11->Type->size[1];

  iobj_11->Type->size[0] = 1;

  iobj_11->Type->size[1] = 5;

  cartes_emxEnsureCapacity_char_T(iobj_11->Type,

    cartesian_waypoints_planner_B.b_kstr_j);

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 5;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_11->Type->data[cartesian_waypoints_planner_B.b_kstr_j] =

      tmp_2[cartesian_waypoints_planner_B.b_kstr_j];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  cartesian_waypoints_planner_B.b_kstr_j = switch_expression->size[0] *

    switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = iobj_11->Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression,

    cartesian_waypoints_planner_B.b_kstr_j);

  cartesian_waypoints_planner_B.loop_ub_g = iobj_11->Type->size[0] *

    iobj_11->Type->size[1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j <=

       cartesian_waypoints_planner_B.loop_ub_g;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    switch_expression->data[cartesian_waypoints_planner_B.b_kstr_j] =

      iobj_11->Type->data[cartesian_waypoints_planner_B.b_kstr_j];

  }



  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 8;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    cartesian_waypoints_planner_B.b_d3[cartesian_waypoints_planner_B.b_kstr_j] =

      tmp_3[cartesian_waypoints_planner_B.b_kstr_j];

  }



  cartesian_waypoints_planner_B.b_bool_i = false;

  if (switch_expression->size[1] == 8) {

    cartesian_waypoints_planner_B.b_kstr_j = 1;

    do {

      exitg1 = 0;

      if (cartesian_waypoints_planner_B.b_kstr_j - 1 < 8) {

        cartesian_waypoints_planner_B.loop_ub_g =

          cartesian_waypoints_planner_B.b_kstr_j - 1;

        if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_g] !=

            cartesian_waypoints_planner_B.b_d3[cartesian_waypoints_planner_B.loop_ub_g])

        {

          exitg1 = 1;

        } else {

          cartesian_waypoints_planner_B.b_kstr_j++;

        }

      } else {

        cartesian_waypoints_planner_B.b_bool_i = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (cartesian_waypoints_planner_B.b_bool_i) {

    cartesian_waypoints_planner_B.b_kstr_j = 0;

  } else {

    for (cartesian_waypoints_planner_B.b_kstr_j = 0;

         cartesian_waypoints_planner_B.b_kstr_j < 9;

         cartesian_waypoints_planner_B.b_kstr_j++) {

      cartesian_waypoints_planner_B.b_e[cartesian_waypoints_planner_B.b_kstr_j] =

        tmp_4[cartesian_waypoints_planner_B.b_kstr_j];

    }



    cartesian_waypoints_planner_B.b_bool_i = false;

    if (switch_expression->size[1] == 9) {

      cartesian_waypoints_planner_B.b_kstr_j = 1;

      do {

        exitg1 = 0;

        if (cartesian_waypoints_planner_B.b_kstr_j - 1 < 9) {

          cartesian_waypoints_planner_B.loop_ub_g =

            cartesian_waypoints_planner_B.b_kstr_j - 1;

          if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_g]

              !=

              cartesian_waypoints_planner_B.b_e[cartesian_waypoints_planner_B.loop_ub_g])

          {

            exitg1 = 1;

          } else {

            cartesian_waypoints_planner_B.b_kstr_j++;

          }

        } else {

          cartesian_waypoints_planner_B.b_bool_i = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (cartesian_waypoints_planner_B.b_bool_i) {

      cartesian_waypoints_planner_B.b_kstr_j = 1;

    } else {

      cartesian_waypoints_planner_B.b_kstr_j = -1;

    }

  }



  switch (cartesian_waypoints_planner_B.b_kstr_j) {

   case 0:

    cartesian_waypoints_planner_B.iv1[0] = 0;

    cartesian_waypoints_planner_B.iv1[1] = 0;

    cartesian_waypoints_planner_B.iv1[2] = 1;

    cartesian_waypoints_planner_B.iv1[3] = 0;

    cartesian_waypoints_planner_B.iv1[4] = 0;

    cartesian_waypoints_planner_B.iv1[5] = 0;

    for (cartesian_waypoints_planner_B.b_kstr_j = 0;

         cartesian_waypoints_planner_B.b_kstr_j < 6;

         cartesian_waypoints_planner_B.b_kstr_j++) {

      cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j]

        =

        cartesian_waypoints_planner_B.iv1[cartesian_waypoints_planner_B.b_kstr_j];

    }



    cartesian_waypoints_planner_B.poslim_data_p[0] = -3.1415926535897931;

    cartesian_waypoints_planner_B.poslim_data_p[1] = 3.1415926535897931;

    iobj_11->VelocityNumber = 1.0;

    iobj_11->PositionNumber = 1.0;

    iobj_11->JointAxisInternal[0] = 0.0;

    iobj_11->JointAxisInternal[1] = 0.0;

    iobj_11->JointAxisInternal[2] = 1.0;

    break;



   case 1:

    cartesian_waypoints_planner_B.iv1[0] = 0;

    cartesian_waypoints_planner_B.iv1[1] = 0;

    cartesian_waypoints_planner_B.iv1[2] = 0;

    cartesian_waypoints_planner_B.iv1[3] = 0;

    cartesian_waypoints_planner_B.iv1[4] = 0;

    cartesian_waypoints_planner_B.iv1[5] = 1;

    for (cartesian_waypoints_planner_B.b_kstr_j = 0;

         cartesian_waypoints_planner_B.b_kstr_j < 6;

         cartesian_waypoints_planner_B.b_kstr_j++) {

      cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j]

        =

        cartesian_waypoints_planner_B.iv1[cartesian_waypoints_planner_B.b_kstr_j];

    }



    cartesian_waypoints_planner_B.poslim_data_p[0] = -0.5;

    cartesian_waypoints_planner_B.poslim_data_p[1] = 0.5;

    iobj_11->VelocityNumber = 1.0;

    iobj_11->PositionNumber = 1.0;

    iobj_11->JointAxisInternal[0] = 0.0;

    iobj_11->JointAxisInternal[1] = 0.0;

    iobj_11->JointAxisInternal[2] = 1.0;

    break;



   default:

    for (cartesian_waypoints_planner_B.b_kstr_j = 0;

         cartesian_waypoints_planner_B.b_kstr_j < 6;

         cartesian_waypoints_planner_B.b_kstr_j++) {

      cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j]

        = 0;

    }



    cartesian_waypoints_planner_B.poslim_data_p[0] = 0.0;

    cartesian_waypoints_planner_B.poslim_data_p[1] = 0.0;

    iobj_11->VelocityNumber = 0.0;

    iobj_11->PositionNumber = 0.0;

    iobj_11->JointAxisInternal[0] = 0.0;

    iobj_11->JointAxisInternal[1] = 0.0;

    iobj_11->JointAxisInternal[2] = 0.0;

    break;

  }



  cartesian_waypoints_planner_B.b_kstr_j = iobj_11->MotionSubspace->size[0] *

    iobj_11->MotionSubspace->size[1];

  iobj_11->MotionSubspace->size[0] = 6;

  iobj_11->MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_11->MotionSubspace,

    cartesian_waypoints_planner_B.b_kstr_j);

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 6;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_11->MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr_j] =

      cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j];

  }



  cartesian_waypoints_planner_B.b_kstr_j = iobj_11->PositionLimitsInternal->

    size[0] * iobj_11->PositionLimitsInternal->size[1];

  iobj_11->PositionLimitsInternal->size[0] = 1;

  iobj_11->PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(iobj_11->PositionLimitsInternal,

    cartesian_waypoints_planner_B.b_kstr_j);

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 2;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_11->PositionLimitsInternal->data[cartesian_waypoints_planner_B.b_kstr_j]

      =

      cartesian_waypoints_planner_B.poslim_data_p[cartesian_waypoints_planner_B.b_kstr_j];

  }



  cartesian_waypoints_planner_B.b_kstr_j = iobj_11->HomePositionInternal->size[0];

  iobj_11->HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_11->HomePositionInternal,

    cartesian_waypoints_planner_B.b_kstr_j);

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 1;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_11->HomePositionInternal->data[0] = 0.0;

  }



  iobj_4->JointInternal = iobj_11;

  iobj_4->Index = -1.0;

  iobj_4->ParentIndex = -1.0;

  obj->Bodies[5] = iobj_4;

  cartesian_waypoints_planner_B.b_kstr_j = iobj_5->NameInternal->size[0] *

    iobj_5->NameInternal->size[1];

  iobj_5->NameInternal->size[0] = 1;

  iobj_5->NameInternal->size[1] = 10;

  cartes_emxEnsureCapacity_char_T(iobj_5->NameInternal,

    cartesian_waypoints_planner_B.b_kstr_j);

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 10;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_5->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_j] =

      tmp_5[cartesian_waypoints_planner_B.b_kstr_j];

  }



  iobj_12->InTree = false;

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 16;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_12->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_j] =

      tmp_0[cartesian_waypoints_planner_B.b_kstr_j];

  }



  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 16;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_12->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_j] =

      tmp_0[cartesian_waypoints_planner_B.b_kstr_j];

  }



  cartesian_waypoints_planner_B.b_kstr_j = iobj_12->NameInternal->size[0] *

    iobj_12->NameInternal->size[1];

  iobj_12->NameInternal->size[0] = 1;

  iobj_12->NameInternal->size[1] = 14;

  cartes_emxEnsureCapacity_char_T(iobj_12->NameInternal,

    cartesian_waypoints_planner_B.b_kstr_j);

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 14;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_12->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_j] =

      tmp_6[cartesian_waypoints_planner_B.b_kstr_j];

  }



  cartesian_waypoints_planner_B.b_kstr_j = iobj_12->Type->size[0] *

    iobj_12->Type->size[1];

  iobj_12->Type->size[0] = 1;

  iobj_12->Type->size[1] = 5;

  cartes_emxEnsureCapacity_char_T(iobj_12->Type,

    cartesian_waypoints_planner_B.b_kstr_j);

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 5;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_12->Type->data[cartesian_waypoints_planner_B.b_kstr_j] =

      tmp_2[cartesian_waypoints_planner_B.b_kstr_j];

  }



  cartesian_waypoints_planner_B.b_kstr_j = switch_expression->size[0] *

    switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = iobj_12->Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression,

    cartesian_waypoints_planner_B.b_kstr_j);

  cartesian_waypoints_planner_B.loop_ub_g = iobj_12->Type->size[0] *

    iobj_12->Type->size[1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j <=

       cartesian_waypoints_planner_B.loop_ub_g;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    switch_expression->data[cartesian_waypoints_planner_B.b_kstr_j] =

      iobj_12->Type->data[cartesian_waypoints_planner_B.b_kstr_j];

  }



  cartesian_waypoints_planner_B.b_bool_i = false;

  if (switch_expression->size[1] == 8) {

    cartesian_waypoints_planner_B.b_kstr_j = 1;

    do {

      exitg1 = 0;

      if (cartesian_waypoints_planner_B.b_kstr_j - 1 < 8) {

        cartesian_waypoints_planner_B.loop_ub_g =

          cartesian_waypoints_planner_B.b_kstr_j - 1;

        if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_g] !=

            cartesian_waypoints_planner_B.b_d3[cartesian_waypoints_planner_B.loop_ub_g])

        {

          exitg1 = 1;

        } else {

          cartesian_waypoints_planner_B.b_kstr_j++;

        }

      } else {

        cartesian_waypoints_planner_B.b_bool_i = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (cartesian_waypoints_planner_B.b_bool_i) {

    cartesian_waypoints_planner_B.b_kstr_j = 0;

  } else {

    for (cartesian_waypoints_planner_B.b_kstr_j = 0;

         cartesian_waypoints_planner_B.b_kstr_j < 9;

         cartesian_waypoints_planner_B.b_kstr_j++) {

      cartesian_waypoints_planner_B.b_e[cartesian_waypoints_planner_B.b_kstr_j] =

        tmp_4[cartesian_waypoints_planner_B.b_kstr_j];

    }



    cartesian_waypoints_planner_B.b_bool_i = false;

    if (switch_expression->size[1] == 9) {

      cartesian_waypoints_planner_B.b_kstr_j = 1;

      do {

        exitg1 = 0;

        if (cartesian_waypoints_planner_B.b_kstr_j - 1 < 9) {

          cartesian_waypoints_planner_B.loop_ub_g =

            cartesian_waypoints_planner_B.b_kstr_j - 1;

          if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_g]

              !=

              cartesian_waypoints_planner_B.b_e[cartesian_waypoints_planner_B.loop_ub_g])

          {

            exitg1 = 1;

          } else {

            cartesian_waypoints_planner_B.b_kstr_j++;

          }

        } else {

          cartesian_waypoints_planner_B.b_bool_i = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (cartesian_waypoints_planner_B.b_bool_i) {

      cartesian_waypoints_planner_B.b_kstr_j = 1;

    } else {

      cartesian_waypoints_planner_B.b_kstr_j = -1;

    }

  }



  switch (cartesian_waypoints_planner_B.b_kstr_j) {

   case 0:

    cartesian_waypoints_planner_B.iv1[0] = 0;

    cartesian_waypoints_planner_B.iv1[1] = 0;

    cartesian_waypoints_planner_B.iv1[2] = 1;

    cartesian_waypoints_planner_B.iv1[3] = 0;

    cartesian_waypoints_planner_B.iv1[4] = 0;

    cartesian_waypoints_planner_B.iv1[5] = 0;

    for (cartesian_waypoints_planner_B.b_kstr_j = 0;

         cartesian_waypoints_planner_B.b_kstr_j < 6;

         cartesian_waypoints_planner_B.b_kstr_j++) {

      cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j]

        =

        cartesian_waypoints_planner_B.iv1[cartesian_waypoints_planner_B.b_kstr_j];

    }



    cartesian_waypoints_planner_B.poslim_data_p[0] = -3.1415926535897931;

    cartesian_waypoints_planner_B.poslim_data_p[1] = 3.1415926535897931;

    iobj_12->VelocityNumber = 1.0;

    iobj_12->PositionNumber = 1.0;

    iobj_12->JointAxisInternal[0] = 0.0;

    iobj_12->JointAxisInternal[1] = 0.0;

    iobj_12->JointAxisInternal[2] = 1.0;

    break;



   case 1:

    cartesian_waypoints_planner_B.iv1[0] = 0;

    cartesian_waypoints_planner_B.iv1[1] = 0;

    cartesian_waypoints_planner_B.iv1[2] = 0;

    cartesian_waypoints_planner_B.iv1[3] = 0;

    cartesian_waypoints_planner_B.iv1[4] = 0;

    cartesian_waypoints_planner_B.iv1[5] = 1;

    for (cartesian_waypoints_planner_B.b_kstr_j = 0;

         cartesian_waypoints_planner_B.b_kstr_j < 6;

         cartesian_waypoints_planner_B.b_kstr_j++) {

      cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j]

        =

        cartesian_waypoints_planner_B.iv1[cartesian_waypoints_planner_B.b_kstr_j];

    }



    cartesian_waypoints_planner_B.poslim_data_p[0] = -0.5;

    cartesian_waypoints_planner_B.poslim_data_p[1] = 0.5;

    iobj_12->VelocityNumber = 1.0;

    iobj_12->PositionNumber = 1.0;

    iobj_12->JointAxisInternal[0] = 0.0;

    iobj_12->JointAxisInternal[1] = 0.0;

    iobj_12->JointAxisInternal[2] = 1.0;

    break;



   default:

    for (cartesian_waypoints_planner_B.b_kstr_j = 0;

         cartesian_waypoints_planner_B.b_kstr_j < 6;

         cartesian_waypoints_planner_B.b_kstr_j++) {

      cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j]

        = 0;

    }



    cartesian_waypoints_planner_B.poslim_data_p[0] = 0.0;

    cartesian_waypoints_planner_B.poslim_data_p[1] = 0.0;

    iobj_12->VelocityNumber = 0.0;

    iobj_12->PositionNumber = 0.0;

    iobj_12->JointAxisInternal[0] = 0.0;

    iobj_12->JointAxisInternal[1] = 0.0;

    iobj_12->JointAxisInternal[2] = 0.0;

    break;

  }



  cartesian_waypoints_planner_B.b_kstr_j = iobj_12->MotionSubspace->size[0] *

    iobj_12->MotionSubspace->size[1];

  iobj_12->MotionSubspace->size[0] = 6;

  iobj_12->MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_12->MotionSubspace,

    cartesian_waypoints_planner_B.b_kstr_j);

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 6;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_12->MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr_j] =

      cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j];

  }



  cartesian_waypoints_planner_B.b_kstr_j = iobj_12->PositionLimitsInternal->

    size[0] * iobj_12->PositionLimitsInternal->size[1];

  iobj_12->PositionLimitsInternal->size[0] = 1;

  iobj_12->PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(iobj_12->PositionLimitsInternal,

    cartesian_waypoints_planner_B.b_kstr_j);

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 2;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_12->PositionLimitsInternal->data[cartesian_waypoints_planner_B.b_kstr_j]

      =

      cartesian_waypoints_planner_B.poslim_data_p[cartesian_waypoints_planner_B.b_kstr_j];

  }



  cartesian_waypoints_planner_B.b_kstr_j = iobj_12->HomePositionInternal->size[0];

  iobj_12->HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_12->HomePositionInternal,

    cartesian_waypoints_planner_B.b_kstr_j);

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 1;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_12->HomePositionInternal->data[0] = 0.0;

  }



  iobj_5->JointInternal = iobj_12;

  iobj_5->Index = -1.0;

  iobj_5->ParentIndex = -1.0;

  obj->Bodies[6] = iobj_5;

  cartesian_waypoints_planner_B.b_kstr_j = iobj_6->NameInternal->size[0] *

    iobj_6->NameInternal->size[1];

  iobj_6->NameInternal->size[0] = 1;

  iobj_6->NameInternal->size[1] = 10;

  cartes_emxEnsureCapacity_char_T(iobj_6->NameInternal,

    cartesian_waypoints_planner_B.b_kstr_j);

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 10;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_6->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_j] =

      tmp_7[cartesian_waypoints_planner_B.b_kstr_j];

  }



  iobj_13->InTree = false;

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 16;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_13->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_j] =

      tmp_0[cartesian_waypoints_planner_B.b_kstr_j];

  }



  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 16;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_13->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_j] =

      tmp_0[cartesian_waypoints_planner_B.b_kstr_j];

  }



  cartesian_waypoints_planner_B.b_kstr_j = iobj_13->NameInternal->size[0] *

    iobj_13->NameInternal->size[1];

  iobj_13->NameInternal->size[0] = 1;

  iobj_13->NameInternal->size[1] = 14;

  cartes_emxEnsureCapacity_char_T(iobj_13->NameInternal,

    cartesian_waypoints_planner_B.b_kstr_j);

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 14;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_13->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_j] =

      tmp_8[cartesian_waypoints_planner_B.b_kstr_j];

  }



  cartesian_waypoints_planner_B.b_kstr_j = iobj_13->Type->size[0] *

    iobj_13->Type->size[1];

  iobj_13->Type->size[0] = 1;

  iobj_13->Type->size[1] = 5;

  cartes_emxEnsureCapacity_char_T(iobj_13->Type,

    cartesian_waypoints_planner_B.b_kstr_j);

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 5;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_13->Type->data[cartesian_waypoints_planner_B.b_kstr_j] =

      tmp_2[cartesian_waypoints_planner_B.b_kstr_j];

  }



  cartesian_waypoints_planner_B.b_kstr_j = switch_expression->size[0] *

    switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = iobj_13->Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression,

    cartesian_waypoints_planner_B.b_kstr_j);

  cartesian_waypoints_planner_B.loop_ub_g = iobj_13->Type->size[0] *

    iobj_13->Type->size[1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j <=

       cartesian_waypoints_planner_B.loop_ub_g;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    switch_expression->data[cartesian_waypoints_planner_B.b_kstr_j] =

      iobj_13->Type->data[cartesian_waypoints_planner_B.b_kstr_j];

  }



  cartesian_waypoints_planner_B.b_bool_i = false;

  if (switch_expression->size[1] == 8) {

    cartesian_waypoints_planner_B.b_kstr_j = 1;

    do {

      exitg1 = 0;

      if (cartesian_waypoints_planner_B.b_kstr_j - 1 < 8) {

        cartesian_waypoints_planner_B.loop_ub_g =

          cartesian_waypoints_planner_B.b_kstr_j - 1;

        if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_g] !=

            cartesian_waypoints_planner_B.b_d3[cartesian_waypoints_planner_B.loop_ub_g])

        {

          exitg1 = 1;

        } else {

          cartesian_waypoints_planner_B.b_kstr_j++;

        }

      } else {

        cartesian_waypoints_planner_B.b_bool_i = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (cartesian_waypoints_planner_B.b_bool_i) {

    cartesian_waypoints_planner_B.b_kstr_j = 0;

  } else {

    for (cartesian_waypoints_planner_B.b_kstr_j = 0;

         cartesian_waypoints_planner_B.b_kstr_j < 9;

         cartesian_waypoints_planner_B.b_kstr_j++) {

      cartesian_waypoints_planner_B.b_e[cartesian_waypoints_planner_B.b_kstr_j] =

        tmp_4[cartesian_waypoints_planner_B.b_kstr_j];

    }



    cartesian_waypoints_planner_B.b_bool_i = false;

    if (switch_expression->size[1] == 9) {

      cartesian_waypoints_planner_B.b_kstr_j = 1;

      do {

        exitg1 = 0;

        if (cartesian_waypoints_planner_B.b_kstr_j - 1 < 9) {

          cartesian_waypoints_planner_B.loop_ub_g =

            cartesian_waypoints_planner_B.b_kstr_j - 1;

          if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_g]

              !=

              cartesian_waypoints_planner_B.b_e[cartesian_waypoints_planner_B.loop_ub_g])

          {

            exitg1 = 1;

          } else {

            cartesian_waypoints_planner_B.b_kstr_j++;

          }

        } else {

          cartesian_waypoints_planner_B.b_bool_i = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (cartesian_waypoints_planner_B.b_bool_i) {

      cartesian_waypoints_planner_B.b_kstr_j = 1;

    } else {

      cartesian_waypoints_planner_B.b_kstr_j = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (cartesian_waypoints_planner_B.b_kstr_j) {

   case 0:

    cartesian_waypoints_planner_B.iv1[0] = 0;

    cartesian_waypoints_planner_B.iv1[1] = 0;

    cartesian_waypoints_planner_B.iv1[2] = 1;

    cartesian_waypoints_planner_B.iv1[3] = 0;

    cartesian_waypoints_planner_B.iv1[4] = 0;

    cartesian_waypoints_planner_B.iv1[5] = 0;

    for (cartesian_waypoints_planner_B.b_kstr_j = 0;

         cartesian_waypoints_planner_B.b_kstr_j < 6;

         cartesian_waypoints_planner_B.b_kstr_j++) {

      cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j]

        =

        cartesian_waypoints_planner_B.iv1[cartesian_waypoints_planner_B.b_kstr_j];

    }



    cartesian_waypoints_planner_B.poslim_data_p[0] = -3.1415926535897931;

    cartesian_waypoints_planner_B.poslim_data_p[1] = 3.1415926535897931;

    iobj_13->VelocityNumber = 1.0;

    iobj_13->PositionNumber = 1.0;

    iobj_13->JointAxisInternal[0] = 0.0;

    iobj_13->JointAxisInternal[1] = 0.0;

    iobj_13->JointAxisInternal[2] = 1.0;

    break;



   case 1:

    cartesian_waypoints_planner_B.iv1[0] = 0;

    cartesian_waypoints_planner_B.iv1[1] = 0;

    cartesian_waypoints_planner_B.iv1[2] = 0;

    cartesian_waypoints_planner_B.iv1[3] = 0;

    cartesian_waypoints_planner_B.iv1[4] = 0;

    cartesian_waypoints_planner_B.iv1[5] = 1;

    for (cartesian_waypoints_planner_B.b_kstr_j = 0;

         cartesian_waypoints_planner_B.b_kstr_j < 6;

         cartesian_waypoints_planner_B.b_kstr_j++) {

      cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j]

        =

        cartesian_waypoints_planner_B.iv1[cartesian_waypoints_planner_B.b_kstr_j];

    }



    cartesian_waypoints_planner_B.poslim_data_p[0] = -0.5;

    cartesian_waypoints_planner_B.poslim_data_p[1] = 0.5;

    iobj_13->VelocityNumber = 1.0;

    iobj_13->PositionNumber = 1.0;

    iobj_13->JointAxisInternal[0] = 0.0;

    iobj_13->JointAxisInternal[1] = 0.0;

    iobj_13->JointAxisInternal[2] = 1.0;

    break;



   default:

    for (cartesian_waypoints_planner_B.b_kstr_j = 0;

         cartesian_waypoints_planner_B.b_kstr_j < 6;

         cartesian_waypoints_planner_B.b_kstr_j++) {

      cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j]

        = 0;

    }



    cartesian_waypoints_planner_B.poslim_data_p[0] = 0.0;

    cartesian_waypoints_planner_B.poslim_data_p[1] = 0.0;

    iobj_13->VelocityNumber = 0.0;

    iobj_13->PositionNumber = 0.0;

    iobj_13->JointAxisInternal[0] = 0.0;

    iobj_13->JointAxisInternal[1] = 0.0;

    iobj_13->JointAxisInternal[2] = 0.0;

    break;

  }



  cartesian_waypoints_planner_B.b_kstr_j = iobj_13->MotionSubspace->size[0] *

    iobj_13->MotionSubspace->size[1];

  iobj_13->MotionSubspace->size[0] = 6;

  iobj_13->MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_13->MotionSubspace,

    cartesian_waypoints_planner_B.b_kstr_j);

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 6;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_13->MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr_j] =

      cartesian_waypoints_planner_B.msubspace_data_p[cartesian_waypoints_planner_B.b_kstr_j];

  }



  cartesian_waypoints_planner_B.b_kstr_j = iobj_13->PositionLimitsInternal->

    size[0] * iobj_13->PositionLimitsInternal->size[1];

  iobj_13->PositionLimitsInternal->size[0] = 1;

  iobj_13->PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(iobj_13->PositionLimitsInternal,

    cartesian_waypoints_planner_B.b_kstr_j);

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 2;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_13->PositionLimitsInternal->data[cartesian_waypoints_planner_B.b_kstr_j]

      =

      cartesian_waypoints_planner_B.poslim_data_p[cartesian_waypoints_planner_B.b_kstr_j];

  }



  cartesian_waypoints_planner_B.b_kstr_j = iobj_13->HomePositionInternal->size[0];

  iobj_13->HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_13->HomePositionInternal,

    cartesian_waypoints_planner_B.b_kstr_j);

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 1;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    iobj_13->HomePositionInternal->data[0] = 0.0;

  }



  iobj_6->JointInternal = iobj_13;

  iobj_6->Index = -1.0;

  iobj_6->ParentIndex = -1.0;

  obj->Bodies[7] = iobj_6;

  obj->NumBodies = 0.0;

  obj->NumNonFixedBodies = 0.0;

  obj->PositionNumber = 0.0;

  obj->VelocityNumber = 0.0;

  cartesian_waypoints_planne_rand(cartesian_waypoints_planner_B.unusedExpr_l);

  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 8;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    obj->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_j] = 0.0;

  }



  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 8;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    obj->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_j + 8] = -1.0;

  }



  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 8;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    obj->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_j] = 0.0;

  }



  for (cartesian_waypoints_planner_B.b_kstr_j = 0;

       cartesian_waypoints_planner_B.b_kstr_j < 8;

       cartesian_waypoints_planner_B.b_kstr_j++) {

    obj->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_j + 8] = -1.0;

  }

}



static x_robotics_manip_internal_Rig_T *c_RigidBodyTree_RigidBodyTree_e

  (x_robotics_manip_internal_Rig_T *obj, w_robotics_manip_internal_Rig_T *iobj_0,

   w_robotics_manip_internal_Rig_T *iobj_1, w_robotics_manip_internal_Rig_T

   *iobj_2, w_robotics_manip_internal_Rig_T *iobj_3,

   w_robotics_manip_internal_Rig_T *iobj_4, w_robotics_manip_internal_Rig_T

   *iobj_5, w_robotics_manip_internal_Rig_T *iobj_6,

   c_rigidBodyJoint_cartesian_wa_T *iobj_7, c_rigidBodyJoint_cartesian_wa_T

   *iobj_8, c_rigidBodyJoint_cartesian_wa_T *iobj_9,

   c_rigidBodyJoint_cartesian_wa_T *iobj_10, c_rigidBodyJoint_cartesian_wa_T

   *iobj_11, c_rigidBodyJoint_cartesian_wa_T *iobj_12,

   c_rigidBodyJoint_cartesian_wa_T *iobj_13, c_rigidBodyJoint_cartesian_wa_T

   *iobj_14, c_rigidBodyJoint_cartesian_wa_T *iobj_15,

   w_robotics_manip_internal_Rig_T *iobj_16)

{

  x_robotics_manip_internal_Rig_T *b_obj;

  emxArray_char_T_cartesian_way_T *switch_expression;

  static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1

  };



  static const char_T tmp_0[8] = { 'b', 'a', 's', 'e', '_', 'j', 'n', 't' };



  static const char_T tmp_1[5] = { 'f', 'i', 'x', 'e', 'd' };



  static const char_T tmp_2[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_3[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  int32_T exitg1;

  b_obj = obj;

  cartesian_waypoints_planner_B.b_kstr_o = obj->Base.NameInternal->size[0] *

    obj->Base.NameInternal->size[1];

  obj->Base.NameInternal->size[0] = 1;

  obj->Base.NameInternal->size[1] = 4;

  cartes_emxEnsureCapacity_char_T(obj->Base.NameInternal,

    cartesian_waypoints_planner_B.b_kstr_o);

  obj->Base.NameInternal->data[0] = 'b';

  obj->Base.NameInternal->data[1] = 'a';

  obj->Base.NameInternal->data[2] = 's';

  obj->Base.NameInternal->data[3] = 'e';

  iobj_15->InTree = false;

  for (cartesian_waypoints_planner_B.b_kstr_o = 0;

       cartesian_waypoints_planner_B.b_kstr_o < 16;

       cartesian_waypoints_planner_B.b_kstr_o++) {

    iobj_15->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_o] =

      tmp[cartesian_waypoints_planner_B.b_kstr_o];

  }



  for (cartesian_waypoints_planner_B.b_kstr_o = 0;

       cartesian_waypoints_planner_B.b_kstr_o < 16;

       cartesian_waypoints_planner_B.b_kstr_o++) {

    iobj_15->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_o] =

      tmp[cartesian_waypoints_planner_B.b_kstr_o];

  }



  cartesian_waypoints_planner_B.b_kstr_o = iobj_15->NameInternal->size[0] *

    iobj_15->NameInternal->size[1];

  iobj_15->NameInternal->size[0] = 1;

  iobj_15->NameInternal->size[1] = 8;

  cartes_emxEnsureCapacity_char_T(iobj_15->NameInternal,

    cartesian_waypoints_planner_B.b_kstr_o);

  for (cartesian_waypoints_planner_B.b_kstr_o = 0;

       cartesian_waypoints_planner_B.b_kstr_o < 8;

       cartesian_waypoints_planner_B.b_kstr_o++) {

    iobj_15->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_o] =

      tmp_0[cartesian_waypoints_planner_B.b_kstr_o];

  }



  cartesian_waypoints_planner_B.b_kstr_o = iobj_15->Type->size[0] *

    iobj_15->Type->size[1];

  iobj_15->Type->size[0] = 1;

  iobj_15->Type->size[1] = 5;

  cartes_emxEnsureCapacity_char_T(iobj_15->Type,

    cartesian_waypoints_planner_B.b_kstr_o);

  for (cartesian_waypoints_planner_B.b_kstr_o = 0;

       cartesian_waypoints_planner_B.b_kstr_o < 5;

       cartesian_waypoints_planner_B.b_kstr_o++) {

    iobj_15->Type->data[cartesian_waypoints_planner_B.b_kstr_o] =

      tmp_1[cartesian_waypoints_planner_B.b_kstr_o];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  cartesian_waypoints_planner_B.b_kstr_o = switch_expression->size[0] *

    switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = iobj_15->Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression,

    cartesian_waypoints_planner_B.b_kstr_o);

  cartesian_waypoints_planner_B.loop_ub_a = iobj_15->Type->size[0] *

    iobj_15->Type->size[1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr_o = 0;

       cartesian_waypoints_planner_B.b_kstr_o <=

       cartesian_waypoints_planner_B.loop_ub_a;

       cartesian_waypoints_planner_B.b_kstr_o++) {

    switch_expression->data[cartesian_waypoints_planner_B.b_kstr_o] =

      iobj_15->Type->data[cartesian_waypoints_planner_B.b_kstr_o];

  }



  for (cartesian_waypoints_planner_B.b_kstr_o = 0;

       cartesian_waypoints_planner_B.b_kstr_o < 8;

       cartesian_waypoints_planner_B.b_kstr_o++) {

    cartesian_waypoints_planner_B.b_nr[cartesian_waypoints_planner_B.b_kstr_o] =

      tmp_2[cartesian_waypoints_planner_B.b_kstr_o];

  }



  cartesian_waypoints_planner_B.b_bool_d = false;

  if (switch_expression->size[1] == 8) {

    cartesian_waypoints_planner_B.b_kstr_o = 1;

    do {

      exitg1 = 0;

      if (cartesian_waypoints_planner_B.b_kstr_o - 1 < 8) {

        cartesian_waypoints_planner_B.loop_ub_a =

          cartesian_waypoints_planner_B.b_kstr_o - 1;

        if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_a] !=

            cartesian_waypoints_planner_B.b_nr[cartesian_waypoints_planner_B.loop_ub_a])

        {

          exitg1 = 1;

        } else {

          cartesian_waypoints_planner_B.b_kstr_o++;

        }

      } else {

        cartesian_waypoints_planner_B.b_bool_d = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (cartesian_waypoints_planner_B.b_bool_d) {

    cartesian_waypoints_planner_B.b_kstr_o = 0;

  } else {

    for (cartesian_waypoints_planner_B.b_kstr_o = 0;

         cartesian_waypoints_planner_B.b_kstr_o < 9;

         cartesian_waypoints_planner_B.b_kstr_o++) {

      cartesian_waypoints_planner_B.b_j[cartesian_waypoints_planner_B.b_kstr_o] =

        tmp_3[cartesian_waypoints_planner_B.b_kstr_o];

    }



    cartesian_waypoints_planner_B.b_bool_d = false;

    if (switch_expression->size[1] == 9) {

      cartesian_waypoints_planner_B.b_kstr_o = 1;

      do {

        exitg1 = 0;

        if (cartesian_waypoints_planner_B.b_kstr_o - 1 < 9) {

          cartesian_waypoints_planner_B.loop_ub_a =

            cartesian_waypoints_planner_B.b_kstr_o - 1;

          if (switch_expression->data[cartesian_waypoints_planner_B.loop_ub_a]

              !=

              cartesian_waypoints_planner_B.b_j[cartesian_waypoints_planner_B.loop_ub_a])

          {

            exitg1 = 1;

          } else {

            cartesian_waypoints_planner_B.b_kstr_o++;

          }

        } else {

          cartesian_waypoints_planner_B.b_bool_d = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (cartesian_waypoints_planner_B.b_bool_d) {

      cartesian_waypoints_planner_B.b_kstr_o = 1;

    } else {

      cartesian_waypoints_planner_B.b_kstr_o = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (cartesian_waypoints_planner_B.b_kstr_o) {

   case 0:

    cartesian_waypoints_planner_B.iv[0] = 0;

    cartesian_waypoints_planner_B.iv[1] = 0;

    cartesian_waypoints_planner_B.iv[2] = 1;

    cartesian_waypoints_planner_B.iv[3] = 0;

    cartesian_waypoints_planner_B.iv[4] = 0;

    cartesian_waypoints_planner_B.iv[5] = 0;

    for (cartesian_waypoints_planner_B.b_kstr_o = 0;

         cartesian_waypoints_planner_B.b_kstr_o < 6;

         cartesian_waypoints_planner_B.b_kstr_o++) {

      cartesian_waypoints_planner_B.msubspace_data[cartesian_waypoints_planner_B.b_kstr_o]

        =

        cartesian_waypoints_planner_B.iv[cartesian_waypoints_planner_B.b_kstr_o];

    }



    cartesian_waypoints_planner_B.poslim_data[0] = -3.1415926535897931;

    cartesian_waypoints_planner_B.poslim_data[1] = 3.1415926535897931;

    iobj_15->VelocityNumber = 1.0;

    iobj_15->PositionNumber = 1.0;

    iobj_15->JointAxisInternal[0] = 0.0;

    iobj_15->JointAxisInternal[1] = 0.0;

    iobj_15->JointAxisInternal[2] = 1.0;

    break;



   case 1:

    cartesian_waypoints_planner_B.iv[0] = 0;

    cartesian_waypoints_planner_B.iv[1] = 0;

    cartesian_waypoints_planner_B.iv[2] = 0;

    cartesian_waypoints_planner_B.iv[3] = 0;

    cartesian_waypoints_planner_B.iv[4] = 0;

    cartesian_waypoints_planner_B.iv[5] = 1;

    for (cartesian_waypoints_planner_B.b_kstr_o = 0;

         cartesian_waypoints_planner_B.b_kstr_o < 6;

         cartesian_waypoints_planner_B.b_kstr_o++) {

      cartesian_waypoints_planner_B.msubspace_data[cartesian_waypoints_planner_B.b_kstr_o]

        =

        cartesian_waypoints_planner_B.iv[cartesian_waypoints_planner_B.b_kstr_o];

    }



    cartesian_waypoints_planner_B.poslim_data[0] = -0.5;

    cartesian_waypoints_planner_B.poslim_data[1] = 0.5;

    iobj_15->VelocityNumber = 1.0;

    iobj_15->PositionNumber = 1.0;

    iobj_15->JointAxisInternal[0] = 0.0;

    iobj_15->JointAxisInternal[1] = 0.0;

    iobj_15->JointAxisInternal[2] = 1.0;

    break;



   default:

    for (cartesian_waypoints_planner_B.b_kstr_o = 0;

         cartesian_waypoints_planner_B.b_kstr_o < 6;

         cartesian_waypoints_planner_B.b_kstr_o++) {

      cartesian_waypoints_planner_B.msubspace_data[cartesian_waypoints_planner_B.b_kstr_o]

        = 0;

    }



    cartesian_waypoints_planner_B.poslim_data[0] = 0.0;

    cartesian_waypoints_planner_B.poslim_data[1] = 0.0;

    iobj_15->VelocityNumber = 0.0;

    iobj_15->PositionNumber = 0.0;

    iobj_15->JointAxisInternal[0] = 0.0;

    iobj_15->JointAxisInternal[1] = 0.0;

    iobj_15->JointAxisInternal[2] = 0.0;

    break;

  }



  cartesian_waypoints_planner_B.b_kstr_o = iobj_15->MotionSubspace->size[0] *

    iobj_15->MotionSubspace->size[1];

  iobj_15->MotionSubspace->size[0] = 6;

  iobj_15->MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_15->MotionSubspace,

    cartesian_waypoints_planner_B.b_kstr_o);

  for (cartesian_waypoints_planner_B.b_kstr_o = 0;

       cartesian_waypoints_planner_B.b_kstr_o < 6;

       cartesian_waypoints_planner_B.b_kstr_o++) {

    iobj_15->MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr_o] =

      cartesian_waypoints_planner_B.msubspace_data[cartesian_waypoints_planner_B.b_kstr_o];

  }



  cartesian_waypoints_planner_B.b_kstr_o = iobj_15->PositionLimitsInternal->

    size[0] * iobj_15->PositionLimitsInternal->size[1];

  iobj_15->PositionLimitsInternal->size[0] = 1;

  iobj_15->PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(iobj_15->PositionLimitsInternal,

    cartesian_waypoints_planner_B.b_kstr_o);

  for (cartesian_waypoints_planner_B.b_kstr_o = 0;

       cartesian_waypoints_planner_B.b_kstr_o < 2;

       cartesian_waypoints_planner_B.b_kstr_o++) {

    iobj_15->PositionLimitsInternal->data[cartesian_waypoints_planner_B.b_kstr_o]

      =

      cartesian_waypoints_planner_B.poslim_data[cartesian_waypoints_planner_B.b_kstr_o];

  }



  cartesian_waypoints_planner_B.b_kstr_o = iobj_15->HomePositionInternal->size[0];

  iobj_15->HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_15->HomePositionInternal,

    cartesian_waypoints_planner_B.b_kstr_o);

  for (cartesian_waypoints_planner_B.b_kstr_o = 0;

       cartesian_waypoints_planner_B.b_kstr_o < 1;

       cartesian_waypoints_planner_B.b_kstr_o++) {

    iobj_15->HomePositionInternal->data[0] = 0.0;

  }



  obj->Base.JointInternal = iobj_15;

  obj->Base.Index = -1.0;

  obj->Base.ParentIndex = -1.0;

  obj->Base.Index = 0.0;

  cartesian_waypoints_planne_rand(cartesian_waypoints_planner_B.unusedExpr);

  ca_RigidBodyTree_clearAllBodies(obj, iobj_0, iobj_1, iobj_2, iobj_3, iobj_4,

    iobj_5, iobj_6, iobj_8, iobj_9, iobj_10, iobj_11, iobj_12, iobj_13, iobj_14,

    iobj_7, iobj_16);

  return b_obj;

}



static c_rigidBodyJoint_cartesian_wa_T *c_rigidBodyJoint_rigidBodyJoint

  (c_rigidBodyJoint_cartesian_wa_T *obj, const emxArray_char_T_cartesian_way_T

   *jname, const emxArray_char_T_cartesian_way_T *jtype)

{

  c_rigidBodyJoint_cartesian_wa_T *b_obj;

  emxArray_char_T_cartesian_way_T *switch_expression;

  static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1

  };



  static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  static const char_T tmp_2[5] = { 'f', 'i', 'x', 'e', 'd' };



  static const char_T tmp_3[128] = { '\x00', '\x01', '\x02', '\x03', '\x04',

    '\x05', '\x06', '\x07', '\x08', '\x09', '\x0a', '\x0b', '\x0c', '\x0d',

    '\x0e', '\x0f', '\x10', '\x11', '\x12', '\x13', '\x14', '\x15', '\x16',

    '\x17', '\x18', '\x19', '\x1a', '\x1b', '\x1c', '\x1d', '\x1e', '\x1f', ' ',

    '!', '\"', '#', '$', '%', '&', '\'', '(', ')', '*', '+', ',', '-', '.', '/',

    '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', ':', ';', '<', '=', '>',

    '?', '@', 'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm',

    'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z', '[', '\\',

    ']', '^', '_', '`', 'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h', 'i', 'j', 'k',

    'l', 'm', 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z',

    '{', '|', '}', '~', '\x7f' };



  boolean_T guard1 = false;

  boolean_T guard2 = false;

  boolean_T guard3 = false;

  boolean_T guard4 = false;

  int32_T exitg1;

  boolean_T guard11 = false;

  obj->InTree = false;

  for (cartesian_waypoints_planner_B.minnanb = 0;

       cartesian_waypoints_planner_B.minnanb < 16;

       cartesian_waypoints_planner_B.minnanb++) {

    obj->JointToParentTransform[cartesian_waypoints_planner_B.minnanb] =

      tmp[cartesian_waypoints_planner_B.minnanb];

  }



  for (cartesian_waypoints_planner_B.minnanb = 0;

       cartesian_waypoints_planner_B.minnanb < 16;

       cartesian_waypoints_planner_B.minnanb++) {

    obj->ChildToJointTransform[cartesian_waypoints_planner_B.minnanb] =

      tmp[cartesian_waypoints_planner_B.minnanb];

  }



  b_obj = obj;

  cartesian_waypoints_planner_B.minnanb = obj->NameInternal->size[0] *

    obj->NameInternal->size[1];

  obj->NameInternal->size[0] = 1;

  obj->NameInternal->size[1] = jname->size[1];

  cartes_emxEnsureCapacity_char_T(obj->NameInternal,

    cartesian_waypoints_planner_B.minnanb);

  cartesian_waypoints_planner_B.loop_ub_hs = jname->size[0] * jname->size[1] - 1;

  for (cartesian_waypoints_planner_B.minnanb = 0;

       cartesian_waypoints_planner_B.minnanb <=

       cartesian_waypoints_planner_B.loop_ub_hs;

       cartesian_waypoints_planner_B.minnanb++) {

    obj->NameInternal->data[cartesian_waypoints_planner_B.minnanb] = jname->

      data[cartesian_waypoints_planner_B.minnanb];

  }



  cartesian_waypoints_planner_B.partial_match_size_idx_1 = 0;

  for (cartesian_waypoints_planner_B.minnanb = 0;

       cartesian_waypoints_planner_B.minnanb < 8;

       cartesian_waypoints_planner_B.minnanb++) {

    cartesian_waypoints_planner_B.vstr[cartesian_waypoints_planner_B.minnanb] =

      tmp_0[cartesian_waypoints_planner_B.minnanb];

  }



  guard1 = false;

  guard2 = false;

  guard3 = false;

  guard4 = false;

  if (jtype->size[1] <= 8) {

    cartesian_waypoints_planner_B.loop_ub_hs = jtype->size[1];

    for (cartesian_waypoints_planner_B.minnanb = 0;

         cartesian_waypoints_planner_B.minnanb < 8;

         cartesian_waypoints_planner_B.minnanb++) {

      cartesian_waypoints_planner_B.b_p2[cartesian_waypoints_planner_B.minnanb] =

        tmp_0[cartesian_waypoints_planner_B.minnanb];

    }



    cartesian_waypoints_planner_B.b_bool_iy = false;

    cartesian_waypoints_planner_B.minnanb = jtype->size[1];

    if (cartesian_waypoints_planner_B.minnanb >= 8) {

      cartesian_waypoints_planner_B.minnanb = 8;

    }



    guard11 = false;

    if (cartesian_waypoints_planner_B.loop_ub_hs <=

        cartesian_waypoints_planner_B.minnanb) {

      if (cartesian_waypoints_planner_B.minnanb <

          cartesian_waypoints_planner_B.loop_ub_hs) {

        cartesian_waypoints_planner_B.loop_ub_hs =

          cartesian_waypoints_planner_B.minnanb;

      }



      cartesian_waypoints_planner_B.minnanb =

        cartesian_waypoints_planner_B.loop_ub_hs - 1;

      guard11 = true;

    } else {

      if (jtype->size[1] == 8) {

        cartesian_waypoints_planner_B.minnanb = 7;

        guard11 = true;

      }

    }



    if (guard11) {

      cartesian_waypoints_planner_B.loop_ub_hs = 1;

      do {

        exitg1 = 0;

        if (cartesian_waypoints_planner_B.loop_ub_hs - 1 <=

            cartesian_waypoints_planner_B.minnanb) {

          cartesian_waypoints_planner_B.kstr =

            cartesian_waypoints_planner_B.loop_ub_hs - 1;

          if (tmp_3[static_cast<uint8_T>(jtype->

               data[cartesian_waypoints_planner_B.kstr]) & 127] != tmp_3[

              static_cast<int32_T>

              (cartesian_waypoints_planner_B.b_p2[cartesian_waypoints_planner_B.kstr])])

          {

            exitg1 = 1;

          } else {

            cartesian_waypoints_planner_B.loop_ub_hs++;

          }

        } else {

          cartesian_waypoints_planner_B.b_bool_iy = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (cartesian_waypoints_planner_B.b_bool_iy) {

      if (jtype->size[1] == 8) {

        cartesian_waypoints_planner_B.nmatched = 1;

        cartesian_waypoints_planner_B.partial_match_size_idx_1 = 8;

        for (cartesian_waypoints_planner_B.minnanb = 0;

             cartesian_waypoints_planner_B.minnanb < 8;

             cartesian_waypoints_planner_B.minnanb++) {

          cartesian_waypoints_planner_B.b_jg[cartesian_waypoints_planner_B.minnanb]

            =

            cartesian_waypoints_planner_B.vstr[cartesian_waypoints_planner_B.minnanb];

        }

      } else {

        cartesian_waypoints_planner_B.partial_match_size_idx_1 = 8;

        for (cartesian_waypoints_planner_B.minnanb = 0;

             cartesian_waypoints_planner_B.minnanb < 8;

             cartesian_waypoints_planner_B.minnanb++) {

          cartesian_waypoints_planner_B.partial_match_data[cartesian_waypoints_planner_B.minnanb]

            =

            cartesian_waypoints_planner_B.vstr[cartesian_waypoints_planner_B.minnanb];

        }



        cartesian_waypoints_planner_B.matched = true;

        cartesian_waypoints_planner_B.nmatched = 1;

        guard3 = true;

      }

    } else {

      guard4 = true;

    }

  } else {

    guard4 = true;

  }



  if (guard4) {

    cartesian_waypoints_planner_B.matched = false;

    cartesian_waypoints_planner_B.nmatched = 0;

    guard3 = true;

  }



  if (guard3) {

    for (cartesian_waypoints_planner_B.minnanb = 0;

         cartesian_waypoints_planner_B.minnanb < 9;

         cartesian_waypoints_planner_B.minnanb++) {

      cartesian_waypoints_planner_B.b_vstr[cartesian_waypoints_planner_B.minnanb]

        = tmp_1[cartesian_waypoints_planner_B.minnanb];

    }



    if (jtype->size[1] <= 9) {

      cartesian_waypoints_planner_B.loop_ub_hs = jtype->size[1];

      for (cartesian_waypoints_planner_B.minnanb = 0;

           cartesian_waypoints_planner_B.minnanb < 9;

           cartesian_waypoints_planner_B.minnanb++) {

        cartesian_waypoints_planner_B.b_jg[cartesian_waypoints_planner_B.minnanb]

          = tmp_1[cartesian_waypoints_planner_B.minnanb];

      }



      cartesian_waypoints_planner_B.b_bool_iy = false;

      cartesian_waypoints_planner_B.minnanb = jtype->size[1];

      if (cartesian_waypoints_planner_B.minnanb >= 9) {

        cartesian_waypoints_planner_B.minnanb = 9;

      }



      guard11 = false;

      if (cartesian_waypoints_planner_B.loop_ub_hs <=

          cartesian_waypoints_planner_B.minnanb) {

        if (cartesian_waypoints_planner_B.minnanb <

            cartesian_waypoints_planner_B.loop_ub_hs) {

          cartesian_waypoints_planner_B.loop_ub_hs =

            cartesian_waypoints_planner_B.minnanb;

        }



        cartesian_waypoints_planner_B.minnanb =

          cartesian_waypoints_planner_B.loop_ub_hs - 1;

        guard11 = true;

      } else {

        if (jtype->size[1] == 9) {

          cartesian_waypoints_planner_B.minnanb = 8;

          guard11 = true;

        }

      }



      if (guard11) {

        cartesian_waypoints_planner_B.loop_ub_hs = 1;

        do {

          exitg1 = 0;

          if (cartesian_waypoints_planner_B.loop_ub_hs - 1 <=

              cartesian_waypoints_planner_B.minnanb) {

            cartesian_waypoints_planner_B.kstr =

              cartesian_waypoints_planner_B.loop_ub_hs - 1;

            if (tmp_3[static_cast<uint8_T>(jtype->

                 data[cartesian_waypoints_planner_B.kstr]) & 127] != tmp_3[

                static_cast<int32_T>

                (cartesian_waypoints_planner_B.b_jg[cartesian_waypoints_planner_B.kstr])])

            {

              exitg1 = 1;

            } else {

              cartesian_waypoints_planner_B.loop_ub_hs++;

            }

          } else {

            cartesian_waypoints_planner_B.b_bool_iy = true;

            exitg1 = 1;

          }

        } while (exitg1 == 0);

      }



      if (cartesian_waypoints_planner_B.b_bool_iy) {

        if (jtype->size[1] == 9) {

          cartesian_waypoints_planner_B.nmatched = 1;

          cartesian_waypoints_planner_B.partial_match_size_idx_1 = 9;

          for (cartesian_waypoints_planner_B.minnanb = 0;

               cartesian_waypoints_planner_B.minnanb < 9;

               cartesian_waypoints_planner_B.minnanb++) {

            cartesian_waypoints_planner_B.b_jg[cartesian_waypoints_planner_B.minnanb]

              =

              cartesian_waypoints_planner_B.b_vstr[cartesian_waypoints_planner_B.minnanb];

          }

        } else {

          if (!cartesian_waypoints_planner_B.matched) {

            cartesian_waypoints_planner_B.partial_match_size_idx_1 = 9;

            for (cartesian_waypoints_planner_B.minnanb = 0;

                 cartesian_waypoints_planner_B.minnanb < 9;

                 cartesian_waypoints_planner_B.minnanb++) {

              cartesian_waypoints_planner_B.partial_match_data[cartesian_waypoints_planner_B.minnanb]

                =

                cartesian_waypoints_planner_B.b_vstr[cartesian_waypoints_planner_B.minnanb];

            }

          }



          cartesian_waypoints_planner_B.matched = true;

          cartesian_waypoints_planner_B.nmatched++;

          guard2 = true;

        }

      } else {

        guard2 = true;

      }

    } else {

      guard2 = true;

    }

  }



  if (guard2) {

    for (cartesian_waypoints_planner_B.minnanb = 0;

         cartesian_waypoints_planner_B.minnanb < 5;

         cartesian_waypoints_planner_B.minnanb++) {

      cartesian_waypoints_planner_B.c_vstr[cartesian_waypoints_planner_B.minnanb]

        = tmp_2[cartesian_waypoints_planner_B.minnanb];

    }



    if (jtype->size[1] <= 5) {

      cartesian_waypoints_planner_B.loop_ub_hs = jtype->size[1];

      for (cartesian_waypoints_planner_B.minnanb = 0;

           cartesian_waypoints_planner_B.minnanb < 5;

           cartesian_waypoints_planner_B.minnanb++) {

        cartesian_waypoints_planner_B.b_eg[cartesian_waypoints_planner_B.minnanb]

          = tmp_2[cartesian_waypoints_planner_B.minnanb];

      }



      cartesian_waypoints_planner_B.b_bool_iy = false;

      cartesian_waypoints_planner_B.minnanb = jtype->size[1];

      if (cartesian_waypoints_planner_B.minnanb >= 5) {

        cartesian_waypoints_planner_B.minnanb = 5;

      }



      guard11 = false;

      if (cartesian_waypoints_planner_B.loop_ub_hs <=

          cartesian_waypoints_planner_B.minnanb) {

        if (cartesian_waypoints_planner_B.minnanb <

            cartesian_waypoints_planner_B.loop_ub_hs) {

          cartesian_waypoints_planner_B.loop_ub_hs =

            cartesian_waypoints_planner_B.minnanb;

        }



        cartesian_waypoints_planner_B.minnanb =

          cartesian_waypoints_planner_B.loop_ub_hs - 1;

        guard11 = true;

      } else {

        if (jtype->size[1] == 5) {

          cartesian_waypoints_planner_B.minnanb = 4;

          guard11 = true;

        }

      }



      if (guard11) {

        cartesian_waypoints_planner_B.loop_ub_hs = 1;

        do {

          exitg1 = 0;

          if (cartesian_waypoints_planner_B.loop_ub_hs - 1 <=

              cartesian_waypoints_planner_B.minnanb) {

            cartesian_waypoints_planner_B.kstr =

              cartesian_waypoints_planner_B.loop_ub_hs - 1;

            if (tmp_3[static_cast<uint8_T>(jtype->

                 data[cartesian_waypoints_planner_B.kstr]) & 127] != tmp_3[

                static_cast<int32_T>

                (cartesian_waypoints_planner_B.b_eg[cartesian_waypoints_planner_B.kstr])])

            {

              exitg1 = 1;

            } else {

              cartesian_waypoints_planner_B.loop_ub_hs++;

            }

          } else {

            cartesian_waypoints_planner_B.b_bool_iy = true;

            exitg1 = 1;

          }

        } while (exitg1 == 0);

      }



      if (cartesian_waypoints_planner_B.b_bool_iy) {

        if (jtype->size[1] == 5) {

          cartesian_waypoints_planner_B.nmatched = 1;

          cartesian_waypoints_planner_B.partial_match_size_idx_1 = 5;

          for (cartesian_waypoints_planner_B.minnanb = 0;

               cartesian_waypoints_planner_B.minnanb < 5;

               cartesian_waypoints_planner_B.minnanb++) {

            cartesian_waypoints_planner_B.b_jg[cartesian_waypoints_planner_B.minnanb]

              =

              cartesian_waypoints_planner_B.c_vstr[cartesian_waypoints_planner_B.minnanb];

          }

        } else {

          if (!cartesian_waypoints_planner_B.matched) {

            cartesian_waypoints_planner_B.partial_match_size_idx_1 = 5;

            for (cartesian_waypoints_planner_B.minnanb = 0;

                 cartesian_waypoints_planner_B.minnanb < 5;

                 cartesian_waypoints_planner_B.minnanb++) {

              cartesian_waypoints_planner_B.partial_match_data[cartesian_waypoints_planner_B.minnanb]

                =

                cartesian_waypoints_planner_B.c_vstr[cartesian_waypoints_planner_B.minnanb];

            }

          }



          cartesian_waypoints_planner_B.nmatched++;

          guard1 = true;

        }

      } else {

        guard1 = true;

      }

    } else {

      guard1 = true;

    }

  }



  if (guard1) {

    if (cartesian_waypoints_planner_B.nmatched == 0) {

      cartesian_waypoints_planner_B.partial_match_size_idx_1 = 0;

    } else {

      cartesian_waypoints_planner_B.loop_ub_hs =

        cartesian_waypoints_planner_B.partial_match_size_idx_1 - 1;

      if (0 <= cartesian_waypoints_planner_B.loop_ub_hs) {

        memcpy(&cartesian_waypoints_planner_B.b_jg[0],

               &cartesian_waypoints_planner_B.partial_match_data[0],

               (cartesian_waypoints_planner_B.loop_ub_hs + 1) * sizeof(char_T));

      }

    }

  }



  if ((cartesian_waypoints_planner_B.nmatched == 0) || ((jtype->size[1] == 0) !=

       (cartesian_waypoints_planner_B.partial_match_size_idx_1 == 0))) {

    cartesian_waypoints_planner_B.partial_match_size_idx_1 = 0;

  } else {

    cartesian_waypoints_planner_B.loop_ub_hs =

      cartesian_waypoints_planner_B.partial_match_size_idx_1 - 1;

    if (0 <= cartesian_waypoints_planner_B.loop_ub_hs) {

      memcpy(&cartesian_waypoints_planner_B.partial_match_data[0],

             &cartesian_waypoints_planner_B.b_jg[0],

             (cartesian_waypoints_planner_B.loop_ub_hs + 1) * sizeof(char_T));

    }

  }



  cartesian_waypoints_planner_B.minnanb = obj->Type->size[0] * obj->Type->size[1];

  obj->Type->size[0] = 1;

  obj->Type->size[1] = cartesian_waypoints_planner_B.partial_match_size_idx_1;

  cartes_emxEnsureCapacity_char_T(obj->Type,

    cartesian_waypoints_planner_B.minnanb);

  cartesian_waypoints_planner_B.loop_ub_hs =

    cartesian_waypoints_planner_B.partial_match_size_idx_1 - 1;

  for (cartesian_waypoints_planner_B.minnanb = 0;

       cartesian_waypoints_planner_B.minnanb <=

       cartesian_waypoints_planner_B.loop_ub_hs;

       cartesian_waypoints_planner_B.minnanb++) {

    obj->Type->data[cartesian_waypoints_planner_B.minnanb] =

      cartesian_waypoints_planner_B.partial_match_data[cartesian_waypoints_planner_B.minnanb];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  cartesian_waypoints_planner_B.minnanb = switch_expression->size[0] *

    switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = obj->Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression,

    cartesian_waypoints_planner_B.minnanb);

  cartesian_waypoints_planner_B.loop_ub_hs = obj->Type->size[0] * obj->

    Type->size[1] - 1;

  for (cartesian_waypoints_planner_B.minnanb = 0;

       cartesian_waypoints_planner_B.minnanb <=

       cartesian_waypoints_planner_B.loop_ub_hs;

       cartesian_waypoints_planner_B.minnanb++) {

    switch_expression->data[cartesian_waypoints_planner_B.minnanb] = obj->

      Type->data[cartesian_waypoints_planner_B.minnanb];

  }



  for (cartesian_waypoints_planner_B.minnanb = 0;

       cartesian_waypoints_planner_B.minnanb < 8;

       cartesian_waypoints_planner_B.minnanb++) {

    cartesian_waypoints_planner_B.b_p2[cartesian_waypoints_planner_B.minnanb] =

      tmp_0[cartesian_waypoints_planner_B.minnanb];

  }



  cartesian_waypoints_planner_B.b_bool_iy = false;

  if (switch_expression->size[1] == 8) {

    cartesian_waypoints_planner_B.loop_ub_hs = 1;

    do {

      exitg1 = 0;

      if (cartesian_waypoints_planner_B.loop_ub_hs - 1 < 8) {

        cartesian_waypoints_planner_B.kstr =

          cartesian_waypoints_planner_B.loop_ub_hs - 1;

        if (switch_expression->data[cartesian_waypoints_planner_B.kstr] !=

            cartesian_waypoints_planner_B.b_p2[cartesian_waypoints_planner_B.kstr])

        {

          exitg1 = 1;

        } else {

          cartesian_waypoints_planner_B.loop_ub_hs++;

        }

      } else {

        cartesian_waypoints_planner_B.b_bool_iy = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (cartesian_waypoints_planner_B.b_bool_iy) {

    cartesian_waypoints_planner_B.minnanb = 0;

  } else {

    for (cartesian_waypoints_planner_B.minnanb = 0;

         cartesian_waypoints_planner_B.minnanb < 9;

         cartesian_waypoints_planner_B.minnanb++) {

      cartesian_waypoints_planner_B.b_jg[cartesian_waypoints_planner_B.minnanb] =

        tmp_1[cartesian_waypoints_planner_B.minnanb];

    }



    cartesian_waypoints_planner_B.b_bool_iy = false;

    if (switch_expression->size[1] == 9) {

      cartesian_waypoints_planner_B.loop_ub_hs = 1;

      do {

        exitg1 = 0;

        if (cartesian_waypoints_planner_B.loop_ub_hs - 1 < 9) {

          cartesian_waypoints_planner_B.kstr =

            cartesian_waypoints_planner_B.loop_ub_hs - 1;

          if (switch_expression->data[cartesian_waypoints_planner_B.kstr] !=

              cartesian_waypoints_planner_B.b_jg[cartesian_waypoints_planner_B.kstr])

          {

            exitg1 = 1;

          } else {

            cartesian_waypoints_planner_B.loop_ub_hs++;

          }

        } else {

          cartesian_waypoints_planner_B.b_bool_iy = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (cartesian_waypoints_planner_B.b_bool_iy) {

      cartesian_waypoints_planner_B.minnanb = 1;

    } else {

      cartesian_waypoints_planner_B.minnanb = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (cartesian_waypoints_planner_B.minnanb) {

   case 0:

    cartesian_waypoints_planner_B.iv3[0] = 0;

    cartesian_waypoints_planner_B.iv3[1] = 0;

    cartesian_waypoints_planner_B.iv3[2] = 1;

    cartesian_waypoints_planner_B.iv3[3] = 0;

    cartesian_waypoints_planner_B.iv3[4] = 0;

    cartesian_waypoints_planner_B.iv3[5] = 0;

    for (cartesian_waypoints_planner_B.minnanb = 0;

         cartesian_waypoints_planner_B.minnanb < 6;

         cartesian_waypoints_planner_B.minnanb++) {

      cartesian_waypoints_planner_B.msubspace_data_f[cartesian_waypoints_planner_B.minnanb]

        =

        cartesian_waypoints_planner_B.iv3[cartesian_waypoints_planner_B.minnanb];

    }



    cartesian_waypoints_planner_B.poslim_data_e[0] = -3.1415926535897931;

    cartesian_waypoints_planner_B.poslim_data_e[1] = 3.1415926535897931;

    obj->VelocityNumber = 1.0;

    obj->PositionNumber = 1.0;

    obj->JointAxisInternal[0] = 0.0;

    obj->JointAxisInternal[1] = 0.0;

    obj->JointAxisInternal[2] = 1.0;

    break;



   case 1:

    cartesian_waypoints_planner_B.iv3[0] = 0;

    cartesian_waypoints_planner_B.iv3[1] = 0;

    cartesian_waypoints_planner_B.iv3[2] = 0;

    cartesian_waypoints_planner_B.iv3[3] = 0;

    cartesian_waypoints_planner_B.iv3[4] = 0;

    cartesian_waypoints_planner_B.iv3[5] = 1;

    for (cartesian_waypoints_planner_B.minnanb = 0;

         cartesian_waypoints_planner_B.minnanb < 6;

         cartesian_waypoints_planner_B.minnanb++) {

      cartesian_waypoints_planner_B.msubspace_data_f[cartesian_waypoints_planner_B.minnanb]

        =

        cartesian_waypoints_planner_B.iv3[cartesian_waypoints_planner_B.minnanb];

    }



    cartesian_waypoints_planner_B.poslim_data_e[0] = -0.5;

    cartesian_waypoints_planner_B.poslim_data_e[1] = 0.5;

    obj->VelocityNumber = 1.0;

    obj->PositionNumber = 1.0;

    obj->JointAxisInternal[0] = 0.0;

    obj->JointAxisInternal[1] = 0.0;

    obj->JointAxisInternal[2] = 1.0;

    break;



   default:

    for (cartesian_waypoints_planner_B.minnanb = 0;

         cartesian_waypoints_planner_B.minnanb < 6;

         cartesian_waypoints_planner_B.minnanb++) {

      cartesian_waypoints_planner_B.msubspace_data_f[cartesian_waypoints_planner_B.minnanb]

        = 0;

    }



    cartesian_waypoints_planner_B.poslim_data_e[0] = 0.0;

    cartesian_waypoints_planner_B.poslim_data_e[1] = 0.0;

    obj->VelocityNumber = 0.0;

    obj->PositionNumber = 0.0;

    obj->JointAxisInternal[0] = 0.0;

    obj->JointAxisInternal[1] = 0.0;

    obj->JointAxisInternal[2] = 0.0;

    break;

  }



  cartesian_waypoints_planner_B.minnanb = obj->MotionSubspace->size[0] *

    obj->MotionSubspace->size[1];

  obj->MotionSubspace->size[0] = 6;

  obj->MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(obj->MotionSubspace,

    cartesian_waypoints_planner_B.minnanb);

  for (cartesian_waypoints_planner_B.minnanb = 0;

       cartesian_waypoints_planner_B.minnanb < 6;

       cartesian_waypoints_planner_B.minnanb++) {

    obj->MotionSubspace->data[cartesian_waypoints_planner_B.minnanb] =

      cartesian_waypoints_planner_B.msubspace_data_f[cartesian_waypoints_planner_B.minnanb];

  }



  cartesian_waypoints_planner_B.minnanb = obj->PositionLimitsInternal->size[0] *

    obj->PositionLimitsInternal->size[1];

  obj->PositionLimitsInternal->size[0] = 1;

  obj->PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(obj->PositionLimitsInternal,

    cartesian_waypoints_planner_B.minnanb);

  for (cartesian_waypoints_planner_B.minnanb = 0;

       cartesian_waypoints_planner_B.minnanb < 2;

       cartesian_waypoints_planner_B.minnanb++) {

    obj->PositionLimitsInternal->data[cartesian_waypoints_planner_B.minnanb] =

      cartesian_waypoints_planner_B.poslim_data_e[cartesian_waypoints_planner_B.minnanb];

  }



  cartesian_waypoints_planner_B.minnanb = obj->HomePositionInternal->size[0];

  obj->HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(obj->HomePositionInternal,

    cartesian_waypoints_planner_B.minnanb);

  for (cartesian_waypoints_planner_B.minnanb = 0;

       cartesian_waypoints_planner_B.minnanb < 1;

       cartesian_waypoints_planner_B.minnanb++) {

    obj->HomePositionInternal->data[0] = 0.0;

  }



  return b_obj;

}



static w_robotics_manip_internal_Rig_T *cartesian_waypoi_RigidBody_copy(const

  v_robotics_manip_internal_Rig_T *obj, c_rigidBodyJoint_cartesian_wa_T *iobj_0,

  c_rigidBodyJoint_cartesian_wa_T *iobj_1, w_robotics_manip_internal_Rig_T

  *iobj_2)

{

  w_robotics_manip_internal_Rig_T *newbody;

  c_rigidBodyJoint_cartesian_wa_T *newjoint;

  emxArray_char_T_cartesian_way_T *jtype;

  emxArray_char_T_cartesian_way_T *jname;

  emxArray_real_T_cartesian_way_T *obj_0;

  emxArray_real_T_cartesian_way_T *obj_1;

  emxArray_real_T_cartesian_way_T *obj_2;

  static const int8_T tmp[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1

  };



  static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };



  static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  int32_T exitg1;

  cartesian_waypoi_emxInit_char_T(&jtype, 2);

  cartesian_waypoints_planner_B.b_kstr_b0 = jtype->size[0] * jtype->size[1];

  jtype->size[0] = 1;

  jtype->size[1] = obj->NameInternal->size[1];

  cartes_emxEnsureCapacity_char_T(jtype, cartesian_waypoints_planner_B.b_kstr_b0);

  cartesian_waypoints_planner_B.loop_ub_da = obj->NameInternal->size[0] *

    obj->NameInternal->size[1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 <=

       cartesian_waypoints_planner_B.loop_ub_da;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    jtype->data[cartesian_waypoints_planner_B.b_kstr_b0] = obj->

      NameInternal->data[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  newbody = iobj_2;

  cartesian_waypoints_planner_B.b_kstr_b0 = iobj_2->NameInternal->size[0] *

    iobj_2->NameInternal->size[1];

  iobj_2->NameInternal->size[0] = 1;

  iobj_2->NameInternal->size[1] = jtype->size[1];

  cartes_emxEnsureCapacity_char_T(iobj_2->NameInternal,

    cartesian_waypoints_planner_B.b_kstr_b0);

  cartesian_waypoints_planner_B.loop_ub_da = jtype->size[0] * jtype->size[1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 <=

       cartesian_waypoints_planner_B.loop_ub_da;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    iobj_2->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_b0] =

      jtype->data[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  iobj_0->InTree = false;

  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 < 16;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    iobj_0->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_b0] =

      tmp[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 < 16;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    iobj_0->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_b0] =

      tmp[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  cartesian_waypoints_planner_B.b_kstr_b0 = iobj_0->NameInternal->size[0] *

    iobj_0->NameInternal->size[1];

  iobj_0->NameInternal->size[0] = 1;

  iobj_0->NameInternal->size[1] = jtype->size[1] + 4;

  cartes_emxEnsureCapacity_char_T(iobj_0->NameInternal,

    cartesian_waypoints_planner_B.b_kstr_b0);

  cartesian_waypoints_planner_B.loop_ub_da = jtype->size[0] * jtype->size[1];

  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 <

       cartesian_waypoints_planner_B.loop_ub_da;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    iobj_0->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_b0] =

      jtype->data[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  iobj_0->NameInternal->data[cartesian_waypoints_planner_B.loop_ub_da] = '_';

  iobj_0->NameInternal->data[cartesian_waypoints_planner_B.loop_ub_da + 1] = 'j';

  iobj_0->NameInternal->data[cartesian_waypoints_planner_B.loop_ub_da + 2] = 'n';

  iobj_0->NameInternal->data[cartesian_waypoints_planner_B.loop_ub_da + 3] = 't';

  cartesian_waypoints_planner_B.b_kstr_b0 = iobj_0->Type->size[0] * iobj_0->

    Type->size[1];

  iobj_0->Type->size[0] = 1;

  iobj_0->Type->size[1] = 5;

  cartes_emxEnsureCapacity_char_T(iobj_0->Type,

    cartesian_waypoints_planner_B.b_kstr_b0);

  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 < 5;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    iobj_0->Type->data[cartesian_waypoints_planner_B.b_kstr_b0] =

      tmp_0[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  cartesian_waypoints_planner_B.b_kstr_b0 = jtype->size[0] * jtype->size[1];

  jtype->size[0] = 1;

  jtype->size[1] = iobj_0->Type->size[1];

  cartes_emxEnsureCapacity_char_T(jtype, cartesian_waypoints_planner_B.b_kstr_b0);

  cartesian_waypoints_planner_B.loop_ub_da = iobj_0->Type->size[0] *

    iobj_0->Type->size[1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 <=

       cartesian_waypoints_planner_B.loop_ub_da;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    jtype->data[cartesian_waypoints_planner_B.b_kstr_b0] = iobj_0->Type->

      data[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 < 8;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    cartesian_waypoints_planner_B.b_p[cartesian_waypoints_planner_B.b_kstr_b0] =

      tmp_1[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  cartesian_waypoints_planner_B.b_bool_a = false;

  if (jtype->size[1] == 8) {

    cartesian_waypoints_planner_B.b_kstr_b0 = 1;

    do {

      exitg1 = 0;

      if (cartesian_waypoints_planner_B.b_kstr_b0 - 1 < 8) {

        cartesian_waypoints_planner_B.loop_ub_da =

          cartesian_waypoints_planner_B.b_kstr_b0 - 1;

        if (jtype->data[cartesian_waypoints_planner_B.loop_ub_da] !=

            cartesian_waypoints_planner_B.b_p[cartesian_waypoints_planner_B.loop_ub_da])

        {

          exitg1 = 1;

        } else {

          cartesian_waypoints_planner_B.b_kstr_b0++;

        }

      } else {

        cartesian_waypoints_planner_B.b_bool_a = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (cartesian_waypoints_planner_B.b_bool_a) {

    cartesian_waypoints_planner_B.b_kstr_b0 = 0;

  } else {

    for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

         cartesian_waypoints_planner_B.b_kstr_b0 < 9;

         cartesian_waypoints_planner_B.b_kstr_b0++) {

      cartesian_waypoints_planner_B.b_m0[cartesian_waypoints_planner_B.b_kstr_b0]

        = tmp_2[cartesian_waypoints_planner_B.b_kstr_b0];

    }



    cartesian_waypoints_planner_B.b_bool_a = false;

    if (jtype->size[1] == 9) {

      cartesian_waypoints_planner_B.b_kstr_b0 = 1;

      do {

        exitg1 = 0;

        if (cartesian_waypoints_planner_B.b_kstr_b0 - 1 < 9) {

          cartesian_waypoints_planner_B.loop_ub_da =

            cartesian_waypoints_planner_B.b_kstr_b0 - 1;

          if (jtype->data[cartesian_waypoints_planner_B.loop_ub_da] !=

              cartesian_waypoints_planner_B.b_m0[cartesian_waypoints_planner_B.loop_ub_da])

          {

            exitg1 = 1;

          } else {

            cartesian_waypoints_planner_B.b_kstr_b0++;

          }

        } else {

          cartesian_waypoints_planner_B.b_bool_a = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (cartesian_waypoints_planner_B.b_bool_a) {

      cartesian_waypoints_planner_B.b_kstr_b0 = 1;

    } else {

      cartesian_waypoints_planner_B.b_kstr_b0 = -1;

    }

  }



  switch (cartesian_waypoints_planner_B.b_kstr_b0) {

   case 0:

    cartesian_waypoints_planner_B.iv2[0] = 0;

    cartesian_waypoints_planner_B.iv2[1] = 0;

    cartesian_waypoints_planner_B.iv2[2] = 1;

    cartesian_waypoints_planner_B.iv2[3] = 0;

    cartesian_waypoints_planner_B.iv2[4] = 0;

    cartesian_waypoints_planner_B.iv2[5] = 0;

    for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

         cartesian_waypoints_planner_B.b_kstr_b0 < 6;

         cartesian_waypoints_planner_B.b_kstr_b0++) {

      cartesian_waypoints_planner_B.msubspace_data_pt[cartesian_waypoints_planner_B.b_kstr_b0]

        =

        cartesian_waypoints_planner_B.iv2[cartesian_waypoints_planner_B.b_kstr_b0];

    }



    cartesian_waypoints_planner_B.poslim_data_a[0] = -3.1415926535897931;

    cartesian_waypoints_planner_B.poslim_data_a[1] = 3.1415926535897931;

    iobj_0->VelocityNumber = 1.0;

    iobj_0->PositionNumber = 1.0;

    iobj_0->JointAxisInternal[0] = 0.0;

    iobj_0->JointAxisInternal[1] = 0.0;

    iobj_0->JointAxisInternal[2] = 1.0;

    break;



   case 1:

    cartesian_waypoints_planner_B.iv2[0] = 0;

    cartesian_waypoints_planner_B.iv2[1] = 0;

    cartesian_waypoints_planner_B.iv2[2] = 0;

    cartesian_waypoints_planner_B.iv2[3] = 0;

    cartesian_waypoints_planner_B.iv2[4] = 0;

    cartesian_waypoints_planner_B.iv2[5] = 1;

    for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

         cartesian_waypoints_planner_B.b_kstr_b0 < 6;

         cartesian_waypoints_planner_B.b_kstr_b0++) {

      cartesian_waypoints_planner_B.msubspace_data_pt[cartesian_waypoints_planner_B.b_kstr_b0]

        =

        cartesian_waypoints_planner_B.iv2[cartesian_waypoints_planner_B.b_kstr_b0];

    }



    cartesian_waypoints_planner_B.poslim_data_a[0] = -0.5;

    cartesian_waypoints_planner_B.poslim_data_a[1] = 0.5;

    iobj_0->VelocityNumber = 1.0;

    iobj_0->PositionNumber = 1.0;

    iobj_0->JointAxisInternal[0] = 0.0;

    iobj_0->JointAxisInternal[1] = 0.0;

    iobj_0->JointAxisInternal[2] = 1.0;

    break;



   default:

    for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

         cartesian_waypoints_planner_B.b_kstr_b0 < 6;

         cartesian_waypoints_planner_B.b_kstr_b0++) {

      cartesian_waypoints_planner_B.msubspace_data_pt[cartesian_waypoints_planner_B.b_kstr_b0]

        = 0;

    }



    cartesian_waypoints_planner_B.poslim_data_a[0] = 0.0;

    cartesian_waypoints_planner_B.poslim_data_a[1] = 0.0;

    iobj_0->VelocityNumber = 0.0;

    iobj_0->PositionNumber = 0.0;

    iobj_0->JointAxisInternal[0] = 0.0;

    iobj_0->JointAxisInternal[1] = 0.0;

    iobj_0->JointAxisInternal[2] = 0.0;

    break;

  }



  cartesian_waypoints_planner_B.b_kstr_b0 = iobj_0->MotionSubspace->size[0] *

    iobj_0->MotionSubspace->size[1];

  iobj_0->MotionSubspace->size[0] = 6;

  iobj_0->MotionSubspace->size[1] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_0->MotionSubspace,

    cartesian_waypoints_planner_B.b_kstr_b0);

  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 < 6;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    iobj_0->MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr_b0] =

      cartesian_waypoints_planner_B.msubspace_data_pt[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  cartesian_waypoints_planner_B.b_kstr_b0 = iobj_0->PositionLimitsInternal->

    size[0] * iobj_0->PositionLimitsInternal->size[1];

  iobj_0->PositionLimitsInternal->size[0] = 1;

  iobj_0->PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(iobj_0->PositionLimitsInternal,

    cartesian_waypoints_planner_B.b_kstr_b0);

  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 < 2;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    iobj_0->PositionLimitsInternal->data[cartesian_waypoints_planner_B.b_kstr_b0]

      =

      cartesian_waypoints_planner_B.poslim_data_a[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  cartesian_waypoints_planner_B.b_kstr_b0 = iobj_0->HomePositionInternal->size[0];

  iobj_0->HomePositionInternal->size[0] = 1;

  cartes_emxEnsureCapacity_real_T(iobj_0->HomePositionInternal,

    cartesian_waypoints_planner_B.b_kstr_b0);

  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 < 1;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    iobj_0->HomePositionInternal->data[0] = 0.0;

  }



  iobj_2->JointInternal = iobj_0;

  iobj_2->Index = -1.0;

  iobj_2->ParentIndex = -1.0;

  cartesian_waypoints_planner_B.b_kstr_b0 = jtype->size[0] * jtype->size[1];

  jtype->size[0] = 1;

  jtype->size[1] = obj->JointInternal.Type->size[1];

  cartes_emxEnsureCapacity_char_T(jtype, cartesian_waypoints_planner_B.b_kstr_b0);

  cartesian_waypoints_planner_B.loop_ub_da = obj->JointInternal.Type->size[0] *

    obj->JointInternal.Type->size[1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 <=

       cartesian_waypoints_planner_B.loop_ub_da;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    jtype->data[cartesian_waypoints_planner_B.b_kstr_b0] =

      obj->JointInternal.Type->data[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  cartesian_waypoi_emxInit_char_T(&jname, 2);

  cartesian_waypoints_planner_B.b_kstr_b0 = jname->size[0] * jname->size[1];

  jname->size[0] = 1;

  jname->size[1] = obj->JointInternal.NameInternal->size[1];

  cartes_emxEnsureCapacity_char_T(jname, cartesian_waypoints_planner_B.b_kstr_b0);

  cartesian_waypoints_planner_B.loop_ub_da = obj->

    JointInternal.NameInternal->size[0] * obj->JointInternal.NameInternal->size

    [1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 <=

       cartesian_waypoints_planner_B.loop_ub_da;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    jname->data[cartesian_waypoints_planner_B.b_kstr_b0] =

      obj->JointInternal.NameInternal->

      data[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  newjoint = c_rigidBodyJoint_rigidBodyJoint(iobj_1, jname, jtype);

  cartesian_waypoints_planner_B.b_kstr_b0 = jtype->size[0] * jtype->size[1];

  jtype->size[0] = 1;

  jtype->size[1] = obj->JointInternal.NameInternal->size[1];

  cartes_emxEnsureCapacity_char_T(jtype, cartesian_waypoints_planner_B.b_kstr_b0);

  cartesian_waypoints_planner_B.loop_ub_da = obj->

    JointInternal.NameInternal->size[0] * obj->JointInternal.NameInternal->size

    [1] - 1;

  cartesian_waypoi_emxFree_char_T(&jname);

  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 <=

       cartesian_waypoints_planner_B.loop_ub_da;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    jtype->data[cartesian_waypoints_planner_B.b_kstr_b0] =

      obj->JointInternal.NameInternal->

      data[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  if (jtype->size[1] != 0) {

    cartesian_waypoints_planner_B.b_kstr_b0 = jtype->size[0] * jtype->size[1];

    jtype->size[0] = 1;

    jtype->size[1] = obj->JointInternal.NameInternal->size[1];

    cartes_emxEnsureCapacity_char_T(jtype,

      cartesian_waypoints_planner_B.b_kstr_b0);

    cartesian_waypoints_planner_B.loop_ub_da = obj->

      JointInternal.NameInternal->size[0] * obj->

      JointInternal.NameInternal->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

         cartesian_waypoints_planner_B.b_kstr_b0 <=

         cartesian_waypoints_planner_B.loop_ub_da;

         cartesian_waypoints_planner_B.b_kstr_b0++) {

      jtype->data[cartesian_waypoints_planner_B.b_kstr_b0] =

        obj->JointInternal.NameInternal->

        data[cartesian_waypoints_planner_B.b_kstr_b0];

    }



    if (!newjoint->InTree) {

      cartesian_waypoints_planner_B.b_kstr_b0 = newjoint->NameInternal->size[0] *

        newjoint->NameInternal->size[1];

      newjoint->NameInternal->size[0] = 1;

      newjoint->NameInternal->size[1] = jtype->size[1];

      cartes_emxEnsureCapacity_char_T(newjoint->NameInternal,

        cartesian_waypoints_planner_B.b_kstr_b0);

      cartesian_waypoints_planner_B.loop_ub_da = jtype->size[0] * jtype->size[1]

        - 1;

      for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

           cartesian_waypoints_planner_B.b_kstr_b0 <=

           cartesian_waypoints_planner_B.loop_ub_da;

           cartesian_waypoints_planner_B.b_kstr_b0++) {

        newjoint->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_b0] =

          jtype->data[cartesian_waypoints_planner_B.b_kstr_b0];

      }

    }

  }



  cartesian_waypoi_emxFree_char_T(&jtype);

  cartesian_waypoi_emxInit_real_T(&obj_0, 1);

  cartesian_waypoints_planner_B.loop_ub_da =

    obj->JointInternal.PositionLimitsInternal->size[0] *

    obj->JointInternal.PositionLimitsInternal->size[1];

  cartesian_waypoints_planner_B.b_kstr_b0 = newjoint->

    PositionLimitsInternal->size[0] * newjoint->PositionLimitsInternal->size[1];

  newjoint->PositionLimitsInternal->size[0] =

    obj->JointInternal.PositionLimitsInternal->size[0];

  newjoint->PositionLimitsInternal->size[1] = 2;

  cartes_emxEnsureCapacity_real_T(newjoint->PositionLimitsInternal,

    cartesian_waypoints_planner_B.b_kstr_b0);

  cartesian_waypoints_planner_B.b_kstr_b0 = obj_0->size[0];

  obj_0->size[0] = cartesian_waypoints_planner_B.loop_ub_da;

  cartes_emxEnsureCapacity_real_T(obj_0, cartesian_waypoints_planner_B.b_kstr_b0);

  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 <

       cartesian_waypoints_planner_B.loop_ub_da;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    obj_0->data[cartesian_waypoints_planner_B.b_kstr_b0] =

      obj->JointInternal.PositionLimitsInternal->

      data[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  cartesian_waypoints_planner_B.loop_ub_da = obj_0->size[0];

  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 <

       cartesian_waypoints_planner_B.loop_ub_da;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    newjoint->PositionLimitsInternal->

      data[cartesian_waypoints_planner_B.b_kstr_b0] = obj_0->

      data[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  cartesian_waypoi_emxFree_real_T(&obj_0);

  cartesian_waypoi_emxInit_real_T(&obj_1, 1);

  cartesian_waypoints_planner_B.b_kstr_b0 = obj_1->size[0];

  obj_1->size[0] = obj->JointInternal.HomePositionInternal->size[0];

  cartes_emxEnsureCapacity_real_T(obj_1, cartesian_waypoints_planner_B.b_kstr_b0);

  cartesian_waypoints_planner_B.loop_ub_da =

    obj->JointInternal.HomePositionInternal->size[0];

  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 <

       cartesian_waypoints_planner_B.loop_ub_da;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    obj_1->data[cartesian_waypoints_planner_B.b_kstr_b0] =

      obj->JointInternal.HomePositionInternal->

      data[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  cartesian_waypoints_planner_B.b_kstr_b0 = newjoint->HomePositionInternal->

    size[0];

  newjoint->HomePositionInternal->size[0] = obj_1->size[0];

  cartes_emxEnsureCapacity_real_T(newjoint->HomePositionInternal,

    cartesian_waypoints_planner_B.b_kstr_b0);

  cartesian_waypoints_planner_B.loop_ub_da = obj_1->size[0];

  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 <

       cartesian_waypoints_planner_B.loop_ub_da;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    newjoint->HomePositionInternal->data[cartesian_waypoints_planner_B.b_kstr_b0]

      = obj_1->data[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  cartesian_waypoi_emxFree_real_T(&obj_1);

  cartesian_waypoints_planner_B.obj_idx_0 = obj->

    JointInternal.JointAxisInternal[0];

  cartesian_waypoints_planner_B.obj_idx_1 = obj->

    JointInternal.JointAxisInternal[1];

  cartesian_waypoints_planner_B.obj_idx_2 = obj->

    JointInternal.JointAxisInternal[2];

  newjoint->JointAxisInternal[0] = cartesian_waypoints_planner_B.obj_idx_0;

  newjoint->JointAxisInternal[1] = cartesian_waypoints_planner_B.obj_idx_1;

  newjoint->JointAxisInternal[2] = cartesian_waypoints_planner_B.obj_idx_2;

  cartesian_waypoi_emxInit_real_T(&obj_2, 1);

  cartesian_waypoints_planner_B.loop_ub_da = obj->

    JointInternal.MotionSubspace->size[0] * obj->

    JointInternal.MotionSubspace->size[1];

  cartesian_waypoints_planner_B.b_kstr_b0 = newjoint->MotionSubspace->size[0] *

    newjoint->MotionSubspace->size[1];

  newjoint->MotionSubspace->size[0] = 6;

  newjoint->MotionSubspace->size[1] = obj->JointInternal.MotionSubspace->size[1];

  cartes_emxEnsureCapacity_real_T(newjoint->MotionSubspace,

    cartesian_waypoints_planner_B.b_kstr_b0);

  cartesian_waypoints_planner_B.b_kstr_b0 = obj_2->size[0];

  obj_2->size[0] = cartesian_waypoints_planner_B.loop_ub_da;

  cartes_emxEnsureCapacity_real_T(obj_2, cartesian_waypoints_planner_B.b_kstr_b0);

  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 <

       cartesian_waypoints_planner_B.loop_ub_da;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    obj_2->data[cartesian_waypoints_planner_B.b_kstr_b0] =

      obj->JointInternal.MotionSubspace->

      data[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  cartesian_waypoints_planner_B.loop_ub_da = obj_2->size[0];

  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 <

       cartesian_waypoints_planner_B.loop_ub_da;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    newjoint->MotionSubspace->data[cartesian_waypoints_planner_B.b_kstr_b0] =

      obj_2->data[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  cartesian_waypoi_emxFree_real_T(&obj_2);

  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 < 16;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    cartesian_waypoints_planner_B.obj[cartesian_waypoints_planner_B.b_kstr_b0] =

      obj->

      JointInternal.JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 < 16;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    newjoint->JointToParentTransform[cartesian_waypoints_planner_B.b_kstr_b0] =

      cartesian_waypoints_planner_B.obj[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 < 16;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    cartesian_waypoints_planner_B.obj[cartesian_waypoints_planner_B.b_kstr_b0] =

      obj->

      JointInternal.ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  for (cartesian_waypoints_planner_B.b_kstr_b0 = 0;

       cartesian_waypoints_planner_B.b_kstr_b0 < 16;

       cartesian_waypoints_planner_B.b_kstr_b0++) {

    newjoint->ChildToJointTransform[cartesian_waypoints_planner_B.b_kstr_b0] =

      cartesian_waypoints_planner_B.obj[cartesian_waypoints_planner_B.b_kstr_b0];

  }



  iobj_2->JointInternal = newjoint;

  return newbody;

}



static void cartesian_RigidBodyTree_addBody(x_robotics_manip_internal_Rig_T *obj,

  v_robotics_manip_internal_Rig_T *bodyin, const emxArray_char_T_cartesian_way_T

  *parentName, c_rigidBodyJoint_cartesian_wa_T *iobj_0,

  c_rigidBodyJoint_cartesian_wa_T *iobj_1, w_robotics_manip_internal_Rig_T

  *iobj_2)

{

  w_robotics_manip_internal_Rig_T *body;

  c_rigidBodyJoint_cartesian_wa_T *jnt;

  emxArray_char_T_cartesian_way_T *bname;

  static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };



  boolean_T exitg1;

  int32_T exitg2;

  cartesian_waypoi_emxInit_char_T(&bname, 2);

  cartesian_waypoints_planner_B.pid = -1.0;

  cartesian_waypoints_planner_B.b_kstr_i4 = bname->size[0] * bname->size[1];

  bname->size[0] = 1;

  bname->size[1] = obj->Base.NameInternal->size[1];

  cartes_emxEnsureCapacity_char_T(bname, cartesian_waypoints_planner_B.b_kstr_i4);

  cartesian_waypoints_planner_B.loop_ub_k3 = obj->Base.NameInternal->size[0] *

    obj->Base.NameInternal->size[1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr_i4 = 0;

       cartesian_waypoints_planner_B.b_kstr_i4 <=

       cartesian_waypoints_planner_B.loop_ub_k3;

       cartesian_waypoints_planner_B.b_kstr_i4++) {

    bname->data[cartesian_waypoints_planner_B.b_kstr_i4] =

      obj->Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr_i4];

  }



  if (cartesian_waypoints_plan_strcmp(bname, parentName)) {

    cartesian_waypoints_planner_B.pid = 0.0;

  } else {

    cartesian_waypoints_planner_B.b_index_o = obj->NumBodies;

    cartesian_waypoints_planner_B.b_i_f = 0;

    exitg1 = false;

    while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_f <=

                         static_cast<int32_T>

                         (cartesian_waypoints_planner_B.b_index_o) - 1)) {

      body = obj->Bodies[cartesian_waypoints_planner_B.b_i_f];

      cartesian_waypoints_planner_B.b_kstr_i4 = bname->size[0] * bname->size[1];

      bname->size[0] = 1;

      bname->size[1] = body->NameInternal->size[1];

      cartes_emxEnsureCapacity_char_T(bname,

        cartesian_waypoints_planner_B.b_kstr_i4);

      cartesian_waypoints_planner_B.loop_ub_k3 = body->NameInternal->size[0] *

        body->NameInternal->size[1] - 1;

      for (cartesian_waypoints_planner_B.b_kstr_i4 = 0;

           cartesian_waypoints_planner_B.b_kstr_i4 <=

           cartesian_waypoints_planner_B.loop_ub_k3;

           cartesian_waypoints_planner_B.b_kstr_i4++) {

        bname->data[cartesian_waypoints_planner_B.b_kstr_i4] =

          body->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_i4];

      }



      if (cartesian_waypoints_plan_strcmp(bname, parentName)) {

        cartesian_waypoints_planner_B.pid = static_cast<real_T>

          (cartesian_waypoints_planner_B.b_i_f) + 1.0;

        exitg1 = true;

      } else {

        cartesian_waypoints_planner_B.b_i_f++;

      }

    }

  }



  cartesian_waypoints_planner_B.b_index_o = obj->NumBodies + 1.0;

  body = cartesian_waypoi_RigidBody_copy(bodyin, iobj_1, iobj_0, iobj_2);

  obj->Bodies[static_cast<int32_T>(cartesian_waypoints_planner_B.b_index_o) - 1]

    = body;

  body->Index = cartesian_waypoints_planner_B.b_index_o;

  body->ParentIndex = cartesian_waypoints_planner_B.pid;

  body->JointInternal->InTree = true;

  obj->NumBodies++;

  jnt = body->JointInternal;

  cartesian_waypoints_planner_B.b_kstr_i4 = bname->size[0] * bname->size[1];

  bname->size[0] = 1;

  bname->size[1] = jnt->Type->size[1];

  cartes_emxEnsureCapacity_char_T(bname, cartesian_waypoints_planner_B.b_kstr_i4);

  cartesian_waypoints_planner_B.loop_ub_k3 = jnt->Type->size[0] * jnt->

    Type->size[1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr_i4 = 0;

       cartesian_waypoints_planner_B.b_kstr_i4 <=

       cartesian_waypoints_planner_B.loop_ub_k3;

       cartesian_waypoints_planner_B.b_kstr_i4++) {

    bname->data[cartesian_waypoints_planner_B.b_kstr_i4] = jnt->Type->

      data[cartesian_waypoints_planner_B.b_kstr_i4];

  }



  for (cartesian_waypoints_planner_B.b_kstr_i4 = 0;

       cartesian_waypoints_planner_B.b_kstr_i4 < 5;

       cartesian_waypoints_planner_B.b_kstr_i4++) {

    cartesian_waypoints_planner_B.b_hm[cartesian_waypoints_planner_B.b_kstr_i4] =

      tmp[cartesian_waypoints_planner_B.b_kstr_i4];

  }



  cartesian_waypoints_planner_B.b_bool_l = false;

  if (bname->size[1] == 5) {

    cartesian_waypoints_planner_B.b_kstr_i4 = 1;

    do {

      exitg2 = 0;

      if (cartesian_waypoints_planner_B.b_kstr_i4 - 1 < 5) {

        cartesian_waypoints_planner_B.loop_ub_k3 =

          cartesian_waypoints_planner_B.b_kstr_i4 - 1;

        if (bname->data[cartesian_waypoints_planner_B.loop_ub_k3] !=

            cartesian_waypoints_planner_B.b_hm[cartesian_waypoints_planner_B.loop_ub_k3])

        {

          exitg2 = 1;

        } else {

          cartesian_waypoints_planner_B.b_kstr_i4++;

        }

      } else {

        cartesian_waypoints_planner_B.b_bool_l = true;

        exitg2 = 1;

      }

    } while (exitg2 == 0);

  }



  cartesian_waypoi_emxFree_char_T(&bname);

  if (!cartesian_waypoints_planner_B.b_bool_l) {

    obj->NumNonFixedBodies++;

    jnt = body->JointInternal;

    cartesian_waypoints_planner_B.b_kstr_i4 = static_cast<int32_T>(body->Index)

      - 1;

    obj->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_i4] =

      obj->PositionNumber + 1.0;

    obj->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_i4 + 8] =

      obj->PositionNumber + jnt->PositionNumber;

    jnt = body->JointInternal;

    cartesian_waypoints_planner_B.b_kstr_i4 = static_cast<int32_T>(body->Index)

      - 1;

    obj->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_i4] =

      obj->VelocityNumber + 1.0;

    obj->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_i4 + 8] =

      obj->VelocityNumber + jnt->VelocityNumber;

  } else {

    cartesian_waypoints_planner_B.b_kstr_i4 = static_cast<int32_T>(body->Index);

    obj->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_i4 - 1] = 0.0;

    obj->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_i4 + 7] = -1.0;

    cartesian_waypoints_planner_B.b_kstr_i4 = static_cast<int32_T>(body->Index);

    obj->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_i4 - 1] = 0.0;

    obj->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_i4 + 7] = -1.0;

  }



  jnt = body->JointInternal;

  obj->PositionNumber += jnt->PositionNumber;

  jnt = body->JointInternal;

  obj->VelocityNumber += jnt->VelocityNumber;

}



static void inverseKinematics_set_RigidBody(b_inverseKinematics_cartesian_T *obj,

  y_robotics_manip_internal_Rig_T *rigidbodytree,

  w_robotics_manip_internal_Rig_T *iobj_0, w_robotics_manip_internal_Rig_T

  *iobj_1, w_robotics_manip_internal_Rig_T *iobj_2,

  w_robotics_manip_internal_Rig_T *iobj_3, w_robotics_manip_internal_Rig_T

  *iobj_4, w_robotics_manip_internal_Rig_T *iobj_5,

  w_robotics_manip_internal_Rig_T *iobj_6, w_robotics_manip_internal_Rig_T

  *iobj_7, w_robotics_manip_internal_Rig_T *iobj_8,

  w_robotics_manip_internal_Rig_T *iobj_9, w_robotics_manip_internal_Rig_T

  *iobj_10, w_robotics_manip_internal_Rig_T *iobj_11,

  w_robotics_manip_internal_Rig_T *iobj_12, w_robotics_manip_internal_Rig_T

  *iobj_13, w_robotics_manip_internal_Rig_T *iobj_14,

  c_rigidBodyJoint_cartesian_wa_T *iobj_15, c_rigidBodyJoint_cartesian_wa_T

  *iobj_16, c_rigidBodyJoint_cartesian_wa_T *iobj_17,

  c_rigidBodyJoint_cartesian_wa_T *iobj_18, c_rigidBodyJoint_cartesian_wa_T

  *iobj_19, c_rigidBodyJoint_cartesian_wa_T *iobj_20,

  c_rigidBodyJoint_cartesian_wa_T *iobj_21, c_rigidBodyJoint_cartesian_wa_T

  *iobj_22, c_rigidBodyJoint_cartesian_wa_T *iobj_23,

  c_rigidBodyJoint_cartesian_wa_T *iobj_24, c_rigidBodyJoint_cartesian_wa_T

  *iobj_25, c_rigidBodyJoint_cartesian_wa_T *iobj_26,

  c_rigidBodyJoint_cartesian_wa_T *iobj_27, c_rigidBodyJoint_cartesian_wa_T

  *iobj_28, c_rigidBodyJoint_cartesian_wa_T *iobj_29,

  c_rigidBodyJoint_cartesian_wa_T *iobj_30, c_rigidBodyJoint_cartesian_wa_T

  *iobj_31, c_rigidBodyJoint_cartesian_wa_T *iobj_32,

  c_rigidBodyJoint_cartesian_wa_T *iobj_33, c_rigidBodyJoint_cartesian_wa_T

  *iobj_34, c_rigidBodyJoint_cartesian_wa_T *iobj_35,

  c_rigidBodyJoint_cartesian_wa_T *iobj_36, c_rigidBodyJoint_cartesian_wa_T

  *iobj_37, c_rigidBodyJoint_cartesian_wa_T *iobj_38,

  c_rigidBodyJoint_cartesian_wa_T *iobj_39, w_robotics_manip_internal_Rig_T

  *iobj_40, x_robotics_manip_internal_Rig_T *iobj_41)

{

  x_robotics_manip_internal_Rig_T *newrobot;

  v_robotics_manip_internal_Rig_T *body;

  v_robotics_manip_internal_Rig_T *parent;

  emxArray_char_T_cartesian_way_T *b_basename;

  w_robotics_manip_internal_Rig_T *body_0;

  c_rigidBodyJoint_cartesian_wa_T *jnt;

  emxArray_char_T_cartesian_way_T *bname;

  static const char_T tmp[5] = { 'f', 'i', 'x', 'e', 'd' };



  boolean_T exitg1;

  int32_T exitg2;

  cartesian_waypoi_emxInit_char_T(&b_basename, 2);

  newrobot = c_RigidBodyTree_RigidBodyTree_e(iobj_41, iobj_0, iobj_1, iobj_2,

    iobj_3, iobj_4, iobj_5, iobj_6, iobj_15, iobj_16, iobj_17, iobj_18, iobj_19,

    iobj_20, iobj_21, iobj_22, iobj_39, iobj_40);

  cartesian_waypoints_planner_B.b_kstr_e = b_basename->size[0] *

    b_basename->size[1];

  b_basename->size[0] = 1;

  b_basename->size[1] = rigidbodytree->Base.NameInternal->size[1];

  cartes_emxEnsureCapacity_char_T(b_basename,

    cartesian_waypoints_planner_B.b_kstr_e);

  cartesian_waypoints_planner_B.loop_ub_mx = rigidbodytree->

    Base.NameInternal->size[0] * rigidbodytree->Base.NameInternal->size[1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr_e = 0;

       cartesian_waypoints_planner_B.b_kstr_e <=

       cartesian_waypoints_planner_B.loop_ub_mx;

       cartesian_waypoints_planner_B.b_kstr_e++) {

    b_basename->data[cartesian_waypoints_planner_B.b_kstr_e] =

      rigidbodytree->Base.NameInternal->

      data[cartesian_waypoints_planner_B.b_kstr_e];

  }



  cartesian_waypoi_emxInit_char_T(&bname, 2);

  cartesian_waypoints_planner_B.bid_l = -1.0;

  cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];

  bname->size[0] = 1;

  bname->size[1] = newrobot->Base.NameInternal->size[1];

  cartes_emxEnsureCapacity_char_T(bname, cartesian_waypoints_planner_B.b_kstr_e);

  cartesian_waypoints_planner_B.loop_ub_mx = newrobot->Base.NameInternal->size[0]

    * newrobot->Base.NameInternal->size[1] - 1;

  for (cartesian_waypoints_planner_B.b_kstr_e = 0;

       cartesian_waypoints_planner_B.b_kstr_e <=

       cartesian_waypoints_planner_B.loop_ub_mx;

       cartesian_waypoints_planner_B.b_kstr_e++) {

    bname->data[cartesian_waypoints_planner_B.b_kstr_e] =

      newrobot->Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];

  }



  if (cartesian_waypoints_plan_strcmp(bname, b_basename)) {

    cartesian_waypoints_planner_B.bid_l = 0.0;

  } else {

    cartesian_waypoints_planner_B.b_index = newrobot->NumBodies;

    cartesian_waypoints_planner_B.b_i_c = 0;

    exitg1 = false;

    while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_c <=

                         static_cast<int32_T>

                         (cartesian_waypoints_planner_B.b_index) - 1)) {

      body_0 = newrobot->Bodies[cartesian_waypoints_planner_B.b_i_c];

      cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];

      bname->size[0] = 1;

      bname->size[1] = body_0->NameInternal->size[1];

      cartes_emxEnsureCapacity_char_T(bname,

        cartesian_waypoints_planner_B.b_kstr_e);

      cartesian_waypoints_planner_B.loop_ub_mx = body_0->NameInternal->size[0] *

        body_0->NameInternal->size[1] - 1;

      for (cartesian_waypoints_planner_B.b_kstr_e = 0;

           cartesian_waypoints_planner_B.b_kstr_e <=

           cartesian_waypoints_planner_B.loop_ub_mx;

           cartesian_waypoints_planner_B.b_kstr_e++) {

        bname->data[cartesian_waypoints_planner_B.b_kstr_e] =

          body_0->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];

      }



      if (cartesian_waypoints_plan_strcmp(bname, b_basename)) {

        cartesian_waypoints_planner_B.bid_l = static_cast<real_T>

          (cartesian_waypoints_planner_B.b_i_c) + 1.0;

        exitg1 = true;

      } else {

        cartesian_waypoints_planner_B.b_i_c++;

      }

    }

  }



  if ((!(cartesian_waypoints_planner_B.bid_l == 0.0)) &&

      (cartesian_waypoints_planner_B.bid_l < 0.0)) {

    cartesian_waypoints_planner_B.b_kstr_e = newrobot->Base.NameInternal->size[0]

      * newrobot->Base.NameInternal->size[1];

    newrobot->Base.NameInternal->size[0] = 1;

    newrobot->Base.NameInternal->size[1] = b_basename->size[1];

    cartes_emxEnsureCapacity_char_T(newrobot->Base.NameInternal,

      cartesian_waypoints_planner_B.b_kstr_e);

    cartesian_waypoints_planner_B.loop_ub_mx = b_basename->size[0] *

      b_basename->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_kstr_e = 0;

         cartesian_waypoints_planner_B.b_kstr_e <=

         cartesian_waypoints_planner_B.loop_ub_mx;

         cartesian_waypoints_planner_B.b_kstr_e++) {

      newrobot->Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e] =

        b_basename->data[cartesian_waypoints_planner_B.b_kstr_e];

    }

  }



  if (1.0 <= rigidbodytree->NumBodies) {

    body = rigidbodytree->Bodies[0];

    cartesian_waypoints_planner_B.bid_l = body->ParentIndex;

    if (cartesian_waypoints_planner_B.bid_l > 0.0) {

      parent = rigidbodytree->Bodies[static_cast<int32_T>

        (cartesian_waypoints_planner_B.bid_l) - 1];

    } else {

      parent = &rigidbodytree->Base;

    }



    cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];

    bname->size[0] = 1;

    bname->size[1] = parent->NameInternal->size[1];

    cartes_emxEnsureCapacity_char_T(bname,

      cartesian_waypoints_planner_B.b_kstr_e);

    cartesian_waypoints_planner_B.loop_ub_mx = parent->NameInternal->size[0] *

      parent->NameInternal->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_kstr_e = 0;

         cartesian_waypoints_planner_B.b_kstr_e <=

         cartesian_waypoints_planner_B.loop_ub_mx;

         cartesian_waypoints_planner_B.b_kstr_e++) {

      bname->data[cartesian_waypoints_planner_B.b_kstr_e] = parent->

        NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];

    }



    cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_24, iobj_23,

      iobj_7);

  }



  if (2.0 <= rigidbodytree->NumBodies) {

    body = rigidbodytree->Bodies[1];

    cartesian_waypoints_planner_B.bid_l = body->ParentIndex;

    if (cartesian_waypoints_planner_B.bid_l > 0.0) {

      parent = rigidbodytree->Bodies[static_cast<int32_T>

        (cartesian_waypoints_planner_B.bid_l) - 1];

    } else {

      parent = &rigidbodytree->Base;

    }



    cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];

    bname->size[0] = 1;

    bname->size[1] = parent->NameInternal->size[1];

    cartes_emxEnsureCapacity_char_T(bname,

      cartesian_waypoints_planner_B.b_kstr_e);

    cartesian_waypoints_planner_B.loop_ub_mx = parent->NameInternal->size[0] *

      parent->NameInternal->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_kstr_e = 0;

         cartesian_waypoints_planner_B.b_kstr_e <=

         cartesian_waypoints_planner_B.loop_ub_mx;

         cartesian_waypoints_planner_B.b_kstr_e++) {

      bname->data[cartesian_waypoints_planner_B.b_kstr_e] = parent->

        NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];

    }



    cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_26, iobj_25,

      iobj_8);

  }



  if (3.0 <= rigidbodytree->NumBodies) {

    body = rigidbodytree->Bodies[2];

    cartesian_waypoints_planner_B.bid_l = body->ParentIndex;

    if (cartesian_waypoints_planner_B.bid_l > 0.0) {

      parent = rigidbodytree->Bodies[static_cast<int32_T>

        (cartesian_waypoints_planner_B.bid_l) - 1];

    } else {

      parent = &rigidbodytree->Base;

    }



    cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];

    bname->size[0] = 1;

    bname->size[1] = parent->NameInternal->size[1];

    cartes_emxEnsureCapacity_char_T(bname,

      cartesian_waypoints_planner_B.b_kstr_e);

    cartesian_waypoints_planner_B.loop_ub_mx = parent->NameInternal->size[0] *

      parent->NameInternal->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_kstr_e = 0;

         cartesian_waypoints_planner_B.b_kstr_e <=

         cartesian_waypoints_planner_B.loop_ub_mx;

         cartesian_waypoints_planner_B.b_kstr_e++) {

      bname->data[cartesian_waypoints_planner_B.b_kstr_e] = parent->

        NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];

    }



    cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_28, iobj_27,

      iobj_9);

  }



  if (4.0 <= rigidbodytree->NumBodies) {

    body = rigidbodytree->Bodies[3];

    cartesian_waypoints_planner_B.bid_l = body->ParentIndex;

    if (cartesian_waypoints_planner_B.bid_l > 0.0) {

      parent = rigidbodytree->Bodies[static_cast<int32_T>

        (cartesian_waypoints_planner_B.bid_l) - 1];

    } else {

      parent = &rigidbodytree->Base;

    }



    cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];

    bname->size[0] = 1;

    bname->size[1] = parent->NameInternal->size[1];

    cartes_emxEnsureCapacity_char_T(bname,

      cartesian_waypoints_planner_B.b_kstr_e);

    cartesian_waypoints_planner_B.loop_ub_mx = parent->NameInternal->size[0] *

      parent->NameInternal->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_kstr_e = 0;

         cartesian_waypoints_planner_B.b_kstr_e <=

         cartesian_waypoints_planner_B.loop_ub_mx;

         cartesian_waypoints_planner_B.b_kstr_e++) {

      bname->data[cartesian_waypoints_planner_B.b_kstr_e] = parent->

        NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];

    }



    cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_30, iobj_29,

      iobj_10);

  }



  if (5.0 <= rigidbodytree->NumBodies) {

    body = rigidbodytree->Bodies[4];

    cartesian_waypoints_planner_B.bid_l = body->ParentIndex;

    if (cartesian_waypoints_planner_B.bid_l > 0.0) {

      parent = rigidbodytree->Bodies[static_cast<int32_T>

        (cartesian_waypoints_planner_B.bid_l) - 1];

    } else {

      parent = &rigidbodytree->Base;

    }



    cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];

    bname->size[0] = 1;

    bname->size[1] = parent->NameInternal->size[1];

    cartes_emxEnsureCapacity_char_T(bname,

      cartesian_waypoints_planner_B.b_kstr_e);

    cartesian_waypoints_planner_B.loop_ub_mx = parent->NameInternal->size[0] *

      parent->NameInternal->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_kstr_e = 0;

         cartesian_waypoints_planner_B.b_kstr_e <=

         cartesian_waypoints_planner_B.loop_ub_mx;

         cartesian_waypoints_planner_B.b_kstr_e++) {

      bname->data[cartesian_waypoints_planner_B.b_kstr_e] = parent->

        NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];

    }



    cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_32, iobj_31,

      iobj_11);

  }



  if (6.0 <= rigidbodytree->NumBodies) {

    body = rigidbodytree->Bodies[5];

    cartesian_waypoints_planner_B.bid_l = body->ParentIndex;

    if (cartesian_waypoints_planner_B.bid_l > 0.0) {

      parent = rigidbodytree->Bodies[static_cast<int32_T>

        (cartesian_waypoints_planner_B.bid_l) - 1];

    } else {

      parent = &rigidbodytree->Base;

    }



    cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];

    bname->size[0] = 1;

    bname->size[1] = parent->NameInternal->size[1];

    cartes_emxEnsureCapacity_char_T(bname,

      cartesian_waypoints_planner_B.b_kstr_e);

    cartesian_waypoints_planner_B.loop_ub_mx = parent->NameInternal->size[0] *

      parent->NameInternal->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_kstr_e = 0;

         cartesian_waypoints_planner_B.b_kstr_e <=

         cartesian_waypoints_planner_B.loop_ub_mx;

         cartesian_waypoints_planner_B.b_kstr_e++) {

      bname->data[cartesian_waypoints_planner_B.b_kstr_e] = parent->

        NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];

    }



    cartesian_RigidBodyTree_addBody(newrobot, body, bname, iobj_34, iobj_33,

      iobj_12);

  }



  if (7.0 <= rigidbodytree->NumBodies) {

    body = rigidbodytree->Bodies[6];

    cartesian_waypoints_planner_B.bid_l = body->ParentIndex;

    if (cartesian_waypoints_planner_B.bid_l > 0.0) {

      parent = rigidbodytree->Bodies[static_cast<int32_T>

        (cartesian_waypoints_planner_B.bid_l) - 1];

    } else {

      parent = &rigidbodytree->Base;

    }



    cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];

    bname->size[0] = 1;

    bname->size[1] = parent->NameInternal->size[1];

    cartes_emxEnsureCapacity_char_T(bname,

      cartesian_waypoints_planner_B.b_kstr_e);

    cartesian_waypoints_planner_B.loop_ub_mx = parent->NameInternal->size[0] *

      parent->NameInternal->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_kstr_e = 0;

         cartesian_waypoints_planner_B.b_kstr_e <=

         cartesian_waypoints_planner_B.loop_ub_mx;

         cartesian_waypoints_planner_B.b_kstr_e++) {

      bname->data[cartesian_waypoints_planner_B.b_kstr_e] = parent->

        NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];

    }



    cartesian_waypoints_planner_B.bid_l = -1.0;

    cartesian_waypoints_planner_B.b_kstr_e = b_basename->size[0] *

      b_basename->size[1];

    b_basename->size[0] = 1;

    b_basename->size[1] = newrobot->Base.NameInternal->size[1];

    cartes_emxEnsureCapacity_char_T(b_basename,

      cartesian_waypoints_planner_B.b_kstr_e);

    cartesian_waypoints_planner_B.loop_ub_mx = newrobot->Base.NameInternal->

      size[0] * newrobot->Base.NameInternal->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_kstr_e = 0;

         cartesian_waypoints_planner_B.b_kstr_e <=

         cartesian_waypoints_planner_B.loop_ub_mx;

         cartesian_waypoints_planner_B.b_kstr_e++) {

      b_basename->data[cartesian_waypoints_planner_B.b_kstr_e] =

        newrobot->Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];

    }



    if (cartesian_waypoints_plan_strcmp(b_basename, bname)) {

      cartesian_waypoints_planner_B.bid_l = 0.0;

    } else {

      cartesian_waypoints_planner_B.b_index = newrobot->NumBodies;

      cartesian_waypoints_planner_B.b_i_c = 0;

      exitg1 = false;

      while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_c <=

                           static_cast<int32_T>

                           (cartesian_waypoints_planner_B.b_index) - 1)) {

        body_0 = newrobot->Bodies[cartesian_waypoints_planner_B.b_i_c];

        cartesian_waypoints_planner_B.b_kstr_e = b_basename->size[0] *

          b_basename->size[1];

        b_basename->size[0] = 1;

        b_basename->size[1] = body_0->NameInternal->size[1];

        cartes_emxEnsureCapacity_char_T(b_basename,

          cartesian_waypoints_planner_B.b_kstr_e);

        cartesian_waypoints_planner_B.loop_ub_mx = body_0->NameInternal->size[0]

          * body_0->NameInternal->size[1] - 1;

        for (cartesian_waypoints_planner_B.b_kstr_e = 0;

             cartesian_waypoints_planner_B.b_kstr_e <=

             cartesian_waypoints_planner_B.loop_ub_mx;

             cartesian_waypoints_planner_B.b_kstr_e++) {

          b_basename->data[cartesian_waypoints_planner_B.b_kstr_e] =

            body_0->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];

        }



        if (cartesian_waypoints_plan_strcmp(b_basename, bname)) {

          cartesian_waypoints_planner_B.bid_l = static_cast<real_T>

            (cartesian_waypoints_planner_B.b_i_c) + 1.0;

          exitg1 = true;

        } else {

          cartesian_waypoints_planner_B.b_i_c++;

        }

      }

    }



    cartesian_waypoints_planner_B.b_index = newrobot->NumBodies + 1.0;

    body_0 = cartesian_waypoi_RigidBody_copy(body, iobj_35, iobj_36, iobj_13);

    newrobot->Bodies[static_cast<int32_T>(cartesian_waypoints_planner_B.b_index)

      - 1] = body_0;

    body_0->Index = cartesian_waypoints_planner_B.b_index;

    body_0->ParentIndex = cartesian_waypoints_planner_B.bid_l;

    body_0->JointInternal->InTree = true;

    newrobot->NumBodies++;

    jnt = body_0->JointInternal;

    cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];

    bname->size[0] = 1;

    bname->size[1] = jnt->Type->size[1];

    cartes_emxEnsureCapacity_char_T(bname,

      cartesian_waypoints_planner_B.b_kstr_e);

    cartesian_waypoints_planner_B.loop_ub_mx = jnt->Type->size[0] * jnt->

      Type->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_kstr_e = 0;

         cartesian_waypoints_planner_B.b_kstr_e <=

         cartesian_waypoints_planner_B.loop_ub_mx;

         cartesian_waypoints_planner_B.b_kstr_e++) {

      bname->data[cartesian_waypoints_planner_B.b_kstr_e] = jnt->Type->

        data[cartesian_waypoints_planner_B.b_kstr_e];

    }



    for (cartesian_waypoints_planner_B.b_kstr_e = 0;

         cartesian_waypoints_planner_B.b_kstr_e < 5;

         cartesian_waypoints_planner_B.b_kstr_e++) {

      cartesian_waypoints_planner_B.b_cf[cartesian_waypoints_planner_B.b_kstr_e]

        = tmp[cartesian_waypoints_planner_B.b_kstr_e];

    }



    cartesian_waypoints_planner_B.b_bool_n = false;

    if (bname->size[1] == 5) {

      cartesian_waypoints_planner_B.b_kstr_e = 1;

      do {

        exitg2 = 0;

        if (cartesian_waypoints_planner_B.b_kstr_e - 1 < 5) {

          cartesian_waypoints_planner_B.loop_ub_mx =

            cartesian_waypoints_planner_B.b_kstr_e - 1;

          if (bname->data[cartesian_waypoints_planner_B.loop_ub_mx] !=

              cartesian_waypoints_planner_B.b_cf[cartesian_waypoints_planner_B.loop_ub_mx])

          {

            exitg2 = 1;

          } else {

            cartesian_waypoints_planner_B.b_kstr_e++;

          }

        } else {

          cartesian_waypoints_planner_B.b_bool_n = true;

          exitg2 = 1;

        }

      } while (exitg2 == 0);

    }



    if (!cartesian_waypoints_planner_B.b_bool_n) {

      newrobot->NumNonFixedBodies++;

      jnt = body_0->JointInternal;

      cartesian_waypoints_planner_B.b_kstr_e = static_cast<int32_T>

        (body_0->Index) - 1;

      newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_e] =

        newrobot->PositionNumber + 1.0;

      newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_e + 8] =

        newrobot->PositionNumber + jnt->PositionNumber;

      jnt = body_0->JointInternal;

      cartesian_waypoints_planner_B.b_kstr_e = static_cast<int32_T>

        (body_0->Index) - 1;

      newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_e] =

        newrobot->VelocityNumber + 1.0;

      newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_e + 8] =

        newrobot->VelocityNumber + jnt->VelocityNumber;

    } else {

      cartesian_waypoints_planner_B.b_kstr_e = static_cast<int32_T>

        (body_0->Index);

      newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_e - 1] = 0.0;

      newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_e + 7] =

        -1.0;

      cartesian_waypoints_planner_B.b_kstr_e = static_cast<int32_T>

        (body_0->Index);

      newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_e - 1] = 0.0;

      newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_e + 7] =

        -1.0;

    }



    jnt = body_0->JointInternal;

    newrobot->PositionNumber += jnt->PositionNumber;

    jnt = body_0->JointInternal;

    newrobot->VelocityNumber += jnt->VelocityNumber;

  }



  if (8.0 <= rigidbodytree->NumBodies) {

    body = rigidbodytree->Bodies[7];

    cartesian_waypoints_planner_B.bid_l = body->ParentIndex;

    if (cartesian_waypoints_planner_B.bid_l > 0.0) {

      parent = rigidbodytree->Bodies[static_cast<int32_T>

        (cartesian_waypoints_planner_B.bid_l) - 1];

    } else {

      parent = &rigidbodytree->Base;

    }



    cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];

    bname->size[0] = 1;

    bname->size[1] = parent->NameInternal->size[1];

    cartes_emxEnsureCapacity_char_T(bname,

      cartesian_waypoints_planner_B.b_kstr_e);

    cartesian_waypoints_planner_B.loop_ub_mx = parent->NameInternal->size[0] *

      parent->NameInternal->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_kstr_e = 0;

         cartesian_waypoints_planner_B.b_kstr_e <=

         cartesian_waypoints_planner_B.loop_ub_mx;

         cartesian_waypoints_planner_B.b_kstr_e++) {

      bname->data[cartesian_waypoints_planner_B.b_kstr_e] = parent->

        NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];

    }



    cartesian_waypoints_planner_B.bid_l = -1.0;

    cartesian_waypoints_planner_B.b_kstr_e = b_basename->size[0] *

      b_basename->size[1];

    b_basename->size[0] = 1;

    b_basename->size[1] = newrobot->Base.NameInternal->size[1];

    cartes_emxEnsureCapacity_char_T(b_basename,

      cartesian_waypoints_planner_B.b_kstr_e);

    cartesian_waypoints_planner_B.loop_ub_mx = newrobot->Base.NameInternal->

      size[0] * newrobot->Base.NameInternal->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_kstr_e = 0;

         cartesian_waypoints_planner_B.b_kstr_e <=

         cartesian_waypoints_planner_B.loop_ub_mx;

         cartesian_waypoints_planner_B.b_kstr_e++) {

      b_basename->data[cartesian_waypoints_planner_B.b_kstr_e] =

        newrobot->Base.NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];

    }



    if (cartesian_waypoints_plan_strcmp(b_basename, bname)) {

      cartesian_waypoints_planner_B.bid_l = 0.0;

    } else {

      cartesian_waypoints_planner_B.b_index = newrobot->NumBodies;

      cartesian_waypoints_planner_B.b_i_c = 0;

      exitg1 = false;

      while ((!exitg1) && (cartesian_waypoints_planner_B.b_i_c <=

                           static_cast<int32_T>

                           (cartesian_waypoints_planner_B.b_index) - 1)) {

        body_0 = newrobot->Bodies[cartesian_waypoints_planner_B.b_i_c];

        cartesian_waypoints_planner_B.b_kstr_e = b_basename->size[0] *

          b_basename->size[1];

        b_basename->size[0] = 1;

        b_basename->size[1] = body_0->NameInternal->size[1];

        cartes_emxEnsureCapacity_char_T(b_basename,

          cartesian_waypoints_planner_B.b_kstr_e);

        cartesian_waypoints_planner_B.loop_ub_mx = body_0->NameInternal->size[0]

          * body_0->NameInternal->size[1] - 1;

        for (cartesian_waypoints_planner_B.b_kstr_e = 0;

             cartesian_waypoints_planner_B.b_kstr_e <=

             cartesian_waypoints_planner_B.loop_ub_mx;

             cartesian_waypoints_planner_B.b_kstr_e++) {

          b_basename->data[cartesian_waypoints_planner_B.b_kstr_e] =

            body_0->NameInternal->data[cartesian_waypoints_planner_B.b_kstr_e];

        }



        if (cartesian_waypoints_plan_strcmp(b_basename, bname)) {

          cartesian_waypoints_planner_B.bid_l = static_cast<real_T>

            (cartesian_waypoints_planner_B.b_i_c) + 1.0;

          exitg1 = true;

        } else {

          cartesian_waypoints_planner_B.b_i_c++;

        }

      }

    }



    cartesian_waypoints_planner_B.b_index = newrobot->NumBodies + 1.0;

    body_0 = cartesian_waypoi_RigidBody_copy(body, iobj_37, iobj_38, iobj_14);

    newrobot->Bodies[static_cast<int32_T>(cartesian_waypoints_planner_B.b_index)

      - 1] = body_0;

    body_0->Index = cartesian_waypoints_planner_B.b_index;

    body_0->ParentIndex = cartesian_waypoints_planner_B.bid_l;

    body_0->JointInternal->InTree = true;

    newrobot->NumBodies++;

    jnt = body_0->JointInternal;

    cartesian_waypoints_planner_B.b_kstr_e = bname->size[0] * bname->size[1];

    bname->size[0] = 1;

    bname->size[1] = jnt->Type->size[1];

    cartes_emxEnsureCapacity_char_T(bname,

      cartesian_waypoints_planner_B.b_kstr_e);

    cartesian_waypoints_planner_B.loop_ub_mx = jnt->Type->size[0] * jnt->

      Type->size[1] - 1;

    for (cartesian_waypoints_planner_B.b_kstr_e = 0;

         cartesian_waypoints_planner_B.b_kstr_e <=

         cartesian_waypoints_planner_B.loop_ub_mx;

         cartesian_waypoints_planner_B.b_kstr_e++) {

      bname->data[cartesian_waypoints_planner_B.b_kstr_e] = jnt->Type->

        data[cartesian_waypoints_planner_B.b_kstr_e];

    }



    for (cartesian_waypoints_planner_B.b_kstr_e = 0;

         cartesian_waypoints_planner_B.b_kstr_e < 5;

         cartesian_waypoints_planner_B.b_kstr_e++) {

      cartesian_waypoints_planner_B.b_cf[cartesian_waypoints_planner_B.b_kstr_e]

        = tmp[cartesian_waypoints_planner_B.b_kstr_e];

    }



    cartesian_waypoints_planner_B.b_bool_n = false;

    if (bname->size[1] == 5) {

      cartesian_waypoints_planner_B.b_kstr_e = 1;

      do {

        exitg2 = 0;

        if (cartesian_waypoints_planner_B.b_kstr_e - 1 < 5) {

          cartesian_waypoints_planner_B.loop_ub_mx =

            cartesian_waypoints_planner_B.b_kstr_e - 1;

          if (bname->data[cartesian_waypoints_planner_B.loop_ub_mx] !=

              cartesian_waypoints_planner_B.b_cf[cartesian_waypoints_planner_B.loop_ub_mx])

          {

            exitg2 = 1;

          } else {

            cartesian_waypoints_planner_B.b_kstr_e++;

          }

        } else {

          cartesian_waypoints_planner_B.b_bool_n = true;

          exitg2 = 1;

        }

      } while (exitg2 == 0);

    }



    if (!cartesian_waypoints_planner_B.b_bool_n) {

      newrobot->NumNonFixedBodies++;

      jnt = body_0->JointInternal;

      cartesian_waypoints_planner_B.b_kstr_e = static_cast<int32_T>

        (body_0->Index) - 1;

      newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_e] =

        newrobot->PositionNumber + 1.0;

      newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_e + 8] =

        newrobot->PositionNumber + jnt->PositionNumber;

      jnt = body_0->JointInternal;

      cartesian_waypoints_planner_B.b_kstr_e = static_cast<int32_T>

        (body_0->Index) - 1;

      newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_e] =

        newrobot->VelocityNumber + 1.0;

      newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_e + 8] =

        newrobot->VelocityNumber + jnt->VelocityNumber;

    } else {

      cartesian_waypoints_planner_B.b_kstr_e = static_cast<int32_T>

        (body_0->Index);

      newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_e - 1] = 0.0;

      newrobot->PositionDoFMap[cartesian_waypoints_planner_B.b_kstr_e + 7] =

        -1.0;

      cartesian_waypoints_planner_B.b_kstr_e = static_cast<int32_T>

        (body_0->Index);

      newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_e - 1] = 0.0;

      newrobot->VelocityDoFMap[cartesian_waypoints_planner_B.b_kstr_e + 7] =

        -1.0;

    }



    jnt = body_0->JointInternal;

    newrobot->PositionNumber += jnt->PositionNumber;

    jnt = body_0->JointInternal;

    newrobot->VelocityNumber += jnt->VelocityNumber;

  }



  cartesian_waypoi_emxFree_char_T(&bname);

  cartesian_waypoi_emxFree_char_T(&b_basename);

  obj->RigidBodyTreeInternal = newrobot;

}



static h_robotics_core_internal_Damp_T *DampedBFGSwGradientProjection_D

  (h_robotics_core_internal_Damp_T *obj)

{

  h_robotics_core_internal_Damp_T *b_obj;

  int32_T i;

  static const char_T tmp[22] = { 'B', 'F', 'G', 'S', 'G', 'r', 'a', 'd', 'i',

    'e', 'n', 't', 'P', 'r', 'o', 'j', 'e', 'c', 't', 'i', 'o', 'n' };



  b_obj = obj;

  obj->MaxNumIteration = 1500.0;

  obj->MaxTime = 10.0;

  obj->GradientTolerance = 1.0E-7;

  obj->SolutionTolerance = 1.0E-6;

  obj->ArmijoRuleBeta = 0.4;

  obj->ArmijoRuleSigma = 1.0E-5;

  obj->ConstraintsOn = true;

  obj->RandomRestart = true;

  obj->StepTolerance = 1.0E-14;

  for (i = 0; i < 22; i++) {

    obj->Name[i] = tmp[i];

  }



  obj->ConstraintMatrix->size[0] = 0;

  obj->ConstraintMatrix->size[1] = 0;

  obj->ConstraintBound->size[0] = 0;

  obj->TimeObj.StartTime = -1.0;

  obj->TimeObjInternal.StartTime = -1.0;

  return b_obj;

}



static void emxInitStruct_c_rigidBodyJoint1(c_rigidBodyJoint_cartesian__e_T

  *pStruct)

{

  cartesian_waypoi_emxInit_char_T(&pStruct->Type, 2);

}



static void emxInitStruct_o_robotics_manip_(o_robotics_manip_internal_Rig_T

  *pStruct)

{

  cartesian_waypoi_emxInit_char_T(&pStruct->NameInternal, 2);

  emxInitStruct_c_rigidBodyJoint1(&pStruct->JointInternal);

}



static void emxInitStruct_p_robotics_manip_(p_robotics_manip_internal_Rig_T

  *pStruct)

{

  emxInitStruct_o_robotics_manip_(&pStruct->Base);

}



static void emxInitStruct_robotics_slmani_e(robotics_slmanip_internal_b_e_T

  *pStruct)

{

  emxInitStruct_p_robotics_manip_(&pStruct->TreeInternal);

}



static void emxInitStruct_n_robotics_manip_(n_robotics_manip_internal_Rig_T

  *pStruct)

{

  cartesian_waypoi_emxInit_char_T(&pStruct->NameInternal, 2);

  emxInitStruct_c_rigidBodyJoint1(&pStruct->JointInternal);

}



static n_robotics_manip_internal_Rig_T *RigidBody_RigidBod_evqusngvo0ar

  (n_robotics_manip_internal_Rig_T *obj)

{

  n_robotics_manip_internal_Rig_T *b_obj;

  emxArray_char_T_cartesian_way_T *switch_expression;

  boolean_T b_bool;

  int32_T b_kstr;

  char_T b[8];

  char_T b_0[9];

  int32_T loop_ub;

  static const char_T tmp[13] = { 'e', 'd', 'o', '_', 'b', 'a', 's', 'e', '_',

    'l', 'i', 'n', 'k' };



  static const char_T tmp_0[5] = { 'f', 'i', 'x', 'e', 'd' };



  static const char_T tmp_1[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_2[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  static const real_T tmp_3[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };



  static const real_T tmp_4[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };



  int32_T exitg1;

  b_obj = obj;

  b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];

  obj->NameInternal->size[0] = 1;

  obj->NameInternal->size[1] = 13;

  cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 13; b_kstr++) {

    obj->NameInternal->data[b_kstr] = tmp[b_kstr];

  }



  obj->ParentIndex = 0.0;

  b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];

  obj->JointInternal.Type->size[0] = 1;

  obj->JointInternal.Type->size[1] = 5;

  cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);

  for (b_kstr = 0; b_kstr < 5; b_kstr++) {

    obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = obj->JointInternal.Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]

    - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    b[b_kstr] = tmp_1[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_2[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (b_kstr) {

   case 0:

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   case 1:

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   default:

    obj->JointInternal.PositionNumber = 0.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 0.0;

    break;

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.JointToParentTransform[b_kstr] = tmp_3[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_4[b_kstr];

  }



  obj->JointInternal.JointAxisInternal[0] = 0.0;

  obj->JointInternal.JointAxisInternal[1] = 0.0;

  obj->JointInternal.JointAxisInternal[2] = 0.0;

  return b_obj;

}



static n_robotics_manip_internal_Rig_T *RigidBody_RigidBo_evqusngvo0ar1

  (n_robotics_manip_internal_Rig_T *obj)

{

  n_robotics_manip_internal_Rig_T *b_obj;

  emxArray_char_T_cartesian_way_T *switch_expression;

  boolean_T b_bool;

  int32_T b_kstr;

  char_T b[8];

  char_T b_0[9];

  int32_T loop_ub;

  static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',

    '1' };



  static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  static const real_T tmp_2[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.337, 1.0 };



  static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };



  int32_T exitg1;

  b_obj = obj;

  b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];

  obj->NameInternal->size[0] = 1;

  obj->NameInternal->size[1] = 10;

  cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 10; b_kstr++) {

    obj->NameInternal->data[b_kstr] = tmp[b_kstr];

  }



  obj->ParentIndex = 1.0;

  b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];

  obj->JointInternal.Type->size[0] = 1;

  obj->JointInternal.Type->size[1] = 8;

  cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);

  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = obj->JointInternal.Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]

    - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    b[b_kstr] = tmp_0[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_1[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (b_kstr) {

   case 0:

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   case 1:

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   default:

    obj->JointInternal.PositionNumber = 0.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 0.0;

    break;

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];

  }



  obj->JointInternal.JointAxisInternal[0] = 0.0;

  obj->JointInternal.JointAxisInternal[1] = 0.0;

  obj->JointInternal.JointAxisInternal[2] = 1.0;

  return b_obj;

}



static n_robotics_manip_internal_Rig_T *RigidBody_RigidB_evqusngvo0ar1d

  (n_robotics_manip_internal_Rig_T *obj)

{

  n_robotics_manip_internal_Rig_T *b_obj;

  emxArray_char_T_cartesian_way_T *switch_expression;

  boolean_T b_bool;

  int32_T b_kstr;

  char_T b[8];

  char_T b_0[9];

  int32_T loop_ub;

  static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',

    '2' };



  static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  static const real_T tmp_2[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,

    4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,

    0.0, 0.0, 0.0, 1.0 };



  static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };



  int32_T exitg1;

  b_obj = obj;

  b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];

  obj->NameInternal->size[0] = 1;

  obj->NameInternal->size[1] = 10;

  cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 10; b_kstr++) {

    obj->NameInternal->data[b_kstr] = tmp[b_kstr];

  }



  obj->ParentIndex = 2.0;

  b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];

  obj->JointInternal.Type->size[0] = 1;

  obj->JointInternal.Type->size[1] = 8;

  cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);

  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = obj->JointInternal.Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]

    - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    b[b_kstr] = tmp_0[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_1[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (b_kstr) {

   case 0:

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   case 1:

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   default:

    obj->JointInternal.PositionNumber = 0.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 0.0;

    break;

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];

  }



  obj->JointInternal.JointAxisInternal[0] = 0.0;

  obj->JointInternal.JointAxisInternal[1] = 0.0;

  obj->JointInternal.JointAxisInternal[2] = -1.0;

  return b_obj;

}



static n_robotics_manip_internal_Rig_T *RigidBody_Rigid_evqusngvo0ar1db

  (n_robotics_manip_internal_Rig_T *obj)

{

  n_robotics_manip_internal_Rig_T *b_obj;

  emxArray_char_T_cartesian_way_T *switch_expression;

  boolean_T b_bool;

  int32_T b_kstr;

  char_T b[8];

  char_T b_0[9];

  int32_T loop_ub;

  static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',

    '3' };



  static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  static const real_T tmp_2[16] = { 1.0, 2.0682310711021444E-13,

    2.0682310711021444E-13, 0.0, 2.0682310711021444E-13, -1.0, -0.0, 0.0,

    2.0682310711021444E-13, 4.2775797634723234E-26, -1.0, 0.0, 0.0, 0.2105, 0.0,

    1.0 };



  static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };



  int32_T exitg1;

  b_obj = obj;

  b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];

  obj->NameInternal->size[0] = 1;

  obj->NameInternal->size[1] = 10;

  cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 10; b_kstr++) {

    obj->NameInternal->data[b_kstr] = tmp[b_kstr];

  }



  obj->ParentIndex = 3.0;

  b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];

  obj->JointInternal.Type->size[0] = 1;

  obj->JointInternal.Type->size[1] = 8;

  cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);

  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = obj->JointInternal.Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]

    - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    b[b_kstr] = tmp_0[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_1[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (b_kstr) {

   case 0:

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   case 1:

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   default:

    obj->JointInternal.PositionNumber = 0.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 0.0;

    break;

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];

  }



  obj->JointInternal.JointAxisInternal[0] = 0.0;

  obj->JointInternal.JointAxisInternal[1] = 0.0;

  obj->JointInternal.JointAxisInternal[2] = 1.0;

  return b_obj;

}



static n_robotics_manip_internal_Rig_T *c_RigidBody_Rigid_c

  (n_robotics_manip_internal_Rig_T *obj)

{

  n_robotics_manip_internal_Rig_T *b_obj;

  emxArray_char_T_cartesian_way_T *switch_expression;

  boolean_T b_bool;

  int32_T b_kstr;

  char_T b[8];

  char_T b_0[9];

  int32_T loop_ub;

  static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',

    '4' };



  static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  static const real_T tmp_2[16] = { 1.0, 0.0, -0.0, 0.0, 0.0,

    4.8965888601467475E-12, 1.0, 0.0, 0.0, -1.0, 4.8965888601467475E-12, 0.0,

    0.0, -0.268, 0.0, 1.0 };



  static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };



  int32_T exitg1;

  b_obj = obj;

  b_kstr = obj->NameInternal->size[0] * obj->NameInternal->size[1];

  obj->NameInternal->size[0] = 1;

  obj->NameInternal->size[1] = 10;

  cartes_emxEnsureCapacity_char_T(obj->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 10; b_kstr++) {

    obj->NameInternal->data[b_kstr] = tmp[b_kstr];

  }



  obj->ParentIndex = 4.0;

  b_kstr = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1];

  obj->JointInternal.Type->size[0] = 1;

  obj->JointInternal.Type->size[1] = 8;

  cartes_emxEnsureCapacity_char_T(obj->JointInternal.Type, b_kstr);

  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    obj->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = obj->JointInternal.Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = obj->JointInternal.Type->size[0] * obj->JointInternal.Type->size[1]

    - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = obj->JointInternal.Type->data[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    b[b_kstr] = tmp_0[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_1[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (b_kstr) {

   case 0:

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   case 1:

    obj->JointInternal.PositionNumber = 1.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   default:

    obj->JointInternal.PositionNumber = 0.0;

    obj->JointInternal.JointAxisInternal[0] = 0.0;

    obj->JointInternal.JointAxisInternal[1] = 0.0;

    obj->JointInternal.JointAxisInternal[2] = 0.0;

    break;

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    obj->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];

  }



  obj->JointInternal.JointAxisInternal[0] = 0.0;

  obj->JointInternal.JointAxisInternal[1] = 0.0;

  obj->JointInternal.JointAxisInternal[2] = 1.0;

  return b_obj;

}



static p_robotics_manip_internal_Rig_T *RigidBodyTree_RigidBodyTree_ev

  (p_robotics_manip_internal_Rig_T *obj, n_robotics_manip_internal_Rig_T *iobj_0,

   n_robotics_manip_internal_Rig_T *iobj_1, n_robotics_manip_internal_Rig_T

   *iobj_2, n_robotics_manip_internal_Rig_T *iobj_3,

   n_robotics_manip_internal_Rig_T *iobj_4, n_robotics_manip_internal_Rig_T

   *iobj_5, n_robotics_manip_internal_Rig_T *iobj_6,

   n_robotics_manip_internal_Rig_T *iobj_7)

{

  p_robotics_manip_internal_Rig_T *b_obj;

  o_robotics_manip_internal_Rig_T *obj_0;

  emxArray_char_T_cartesian_way_T *switch_expression;

  boolean_T b_bool;

  int32_T b_kstr;

  char_T b[8];

  char_T b_0[9];

  int32_T loop_ub;

  static const char_T tmp[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',

    '5' };



  static const char_T tmp_0[8] = { 'r', 'e', 'v', 'o', 'l', 'u', 't', 'e' };



  static const char_T tmp_1[9] = { 'p', 'r', 'i', 's', 'm', 'a', 't', 'i', 'c' };



  static const real_T tmp_2[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };



  static const real_T tmp_3[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 };



  static const char_T tmp_4[10] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',

    '6' };



  static const real_T tmp_5[16] = { 1.0, 0.0, -0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,

    0.0, 1.0, 0.0, 0.0, 0.0, 0.1745, 1.0 };



  static const char_T tmp_6[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',

    'e', 'e' };



  static const char_T tmp_7[5] = { 'f', 'i', 'x', 'e', 'd' };



  static const char_T tmp_8[5] = { 'w', 'o', 'r', 'l', 'd' };



  int32_T exitg1;

  b_obj = obj;

  obj->Bodies[0] = RigidBody_RigidBod_evqusngvo0ar(iobj_0);

  obj->Bodies[1] = RigidBody_RigidBo_evqusngvo0ar1(iobj_7);

  obj->Bodies[2] = RigidBody_RigidB_evqusngvo0ar1d(iobj_1);

  obj->Bodies[3] = RigidBody_Rigid_evqusngvo0ar1db(iobj_2);

  obj->Bodies[4] = c_RigidBody_Rigid_c(iobj_3);

  b_kstr = iobj_4->NameInternal->size[0] * iobj_4->NameInternal->size[1];

  iobj_4->NameInternal->size[0] = 1;

  iobj_4->NameInternal->size[1] = 10;

  cartes_emxEnsureCapacity_char_T(iobj_4->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 10; b_kstr++) {

    iobj_4->NameInternal->data[b_kstr] = tmp[b_kstr];

  }



  iobj_4->ParentIndex = 5.0;

  b_kstr = iobj_4->JointInternal.Type->size[0] * iobj_4->

    JointInternal.Type->size[1];

  iobj_4->JointInternal.Type->size[0] = 1;

  iobj_4->JointInternal.Type->size[1] = 8;

  cartes_emxEnsureCapacity_char_T(iobj_4->JointInternal.Type, b_kstr);

  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    iobj_4->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];

  }



  cartesian_waypoi_emxInit_char_T(&switch_expression, 2);

  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = iobj_4->JointInternal.Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = iobj_4->JointInternal.Type->size[0] * iobj_4->

    JointInternal.Type->size[1] - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = iobj_4->JointInternal.Type->data[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    b[b_kstr] = tmp_0[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_1[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  switch (b_kstr) {

   case 0:

    iobj_4->JointInternal.PositionNumber = 1.0;

    iobj_4->JointInternal.JointAxisInternal[0] = 0.0;

    iobj_4->JointInternal.JointAxisInternal[1] = 0.0;

    iobj_4->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   case 1:

    iobj_4->JointInternal.PositionNumber = 1.0;

    iobj_4->JointInternal.JointAxisInternal[0] = 0.0;

    iobj_4->JointInternal.JointAxisInternal[1] = 0.0;

    iobj_4->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   default:

    iobj_4->JointInternal.PositionNumber = 0.0;

    iobj_4->JointInternal.JointAxisInternal[0] = 0.0;

    iobj_4->JointInternal.JointAxisInternal[1] = 0.0;

    iobj_4->JointInternal.JointAxisInternal[2] = 0.0;

    break;

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    iobj_4->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    iobj_4->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];

  }



  iobj_4->JointInternal.JointAxisInternal[0] = 0.0;

  iobj_4->JointInternal.JointAxisInternal[1] = 1.0;

  iobj_4->JointInternal.JointAxisInternal[2] = 0.0;

  obj->Bodies[5] = iobj_4;

  b_kstr = iobj_5->NameInternal->size[0] * iobj_5->NameInternal->size[1];

  iobj_5->NameInternal->size[0] = 1;

  iobj_5->NameInternal->size[1] = 10;

  cartes_emxEnsureCapacity_char_T(iobj_5->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 10; b_kstr++) {

    iobj_5->NameInternal->data[b_kstr] = tmp_4[b_kstr];

  }



  iobj_5->ParentIndex = 6.0;

  b_kstr = iobj_5->JointInternal.Type->size[0] * iobj_5->

    JointInternal.Type->size[1];

  iobj_5->JointInternal.Type->size[0] = 1;

  iobj_5->JointInternal.Type->size[1] = 8;

  cartes_emxEnsureCapacity_char_T(iobj_5->JointInternal.Type, b_kstr);

  for (b_kstr = 0; b_kstr < 8; b_kstr++) {

    iobj_5->JointInternal.Type->data[b_kstr] = tmp_0[b_kstr];

  }



  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = iobj_5->JointInternal.Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = iobj_5->JointInternal.Type->size[0] * iobj_5->

    JointInternal.Type->size[1] - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = iobj_5->JointInternal.Type->data[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_1[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  switch (b_kstr) {

   case 0:

    iobj_5->JointInternal.PositionNumber = 1.0;

    iobj_5->JointInternal.JointAxisInternal[0] = 0.0;

    iobj_5->JointInternal.JointAxisInternal[1] = 0.0;

    iobj_5->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   case 1:

    iobj_5->JointInternal.PositionNumber = 1.0;

    iobj_5->JointInternal.JointAxisInternal[0] = 0.0;

    iobj_5->JointInternal.JointAxisInternal[1] = 0.0;

    iobj_5->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   default:

    iobj_5->JointInternal.PositionNumber = 0.0;

    iobj_5->JointInternal.JointAxisInternal[0] = 0.0;

    iobj_5->JointInternal.JointAxisInternal[1] = 0.0;

    iobj_5->JointInternal.JointAxisInternal[2] = 0.0;

    break;

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    iobj_5->JointInternal.JointToParentTransform[b_kstr] = tmp_5[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    iobj_5->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];

  }



  iobj_5->JointInternal.JointAxisInternal[0] = 0.0;

  iobj_5->JointInternal.JointAxisInternal[1] = 0.0;

  iobj_5->JointInternal.JointAxisInternal[2] = 1.0;

  obj->Bodies[6] = iobj_5;

  b_kstr = iobj_6->NameInternal->size[0] * iobj_6->NameInternal->size[1];

  iobj_6->NameInternal->size[0] = 1;

  iobj_6->NameInternal->size[1] = 11;

  cartes_emxEnsureCapacity_char_T(iobj_6->NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 11; b_kstr++) {

    iobj_6->NameInternal->data[b_kstr] = tmp_6[b_kstr];

  }



  iobj_6->ParentIndex = 7.0;

  b_kstr = iobj_6->JointInternal.Type->size[0] * iobj_6->

    JointInternal.Type->size[1];

  iobj_6->JointInternal.Type->size[0] = 1;

  iobj_6->JointInternal.Type->size[1] = 5;

  cartes_emxEnsureCapacity_char_T(iobj_6->JointInternal.Type, b_kstr);

  for (b_kstr = 0; b_kstr < 5; b_kstr++) {

    iobj_6->JointInternal.Type->data[b_kstr] = tmp_7[b_kstr];

  }



  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = iobj_6->JointInternal.Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = iobj_6->JointInternal.Type->size[0] * iobj_6->

    JointInternal.Type->size[1] - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = iobj_6->JointInternal.Type->data[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_1[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  switch (b_kstr) {

   case 0:

    iobj_6->JointInternal.PositionNumber = 1.0;

    iobj_6->JointInternal.JointAxisInternal[0] = 0.0;

    iobj_6->JointInternal.JointAxisInternal[1] = 0.0;

    iobj_6->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   case 1:

    iobj_6->JointInternal.PositionNumber = 1.0;

    iobj_6->JointInternal.JointAxisInternal[0] = 0.0;

    iobj_6->JointInternal.JointAxisInternal[1] = 0.0;

    iobj_6->JointInternal.JointAxisInternal[2] = 1.0;

    break;



   default:

    iobj_6->JointInternal.PositionNumber = 0.0;

    iobj_6->JointInternal.JointAxisInternal[0] = 0.0;

    iobj_6->JointInternal.JointAxisInternal[1] = 0.0;

    iobj_6->JointInternal.JointAxisInternal[2] = 0.0;

    break;

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    iobj_6->JointInternal.JointToParentTransform[b_kstr] = tmp_2[b_kstr];

  }



  for (b_kstr = 0; b_kstr < 16; b_kstr++) {

    iobj_6->JointInternal.ChildToJointTransform[b_kstr] = tmp_3[b_kstr];

  }



  iobj_6->JointInternal.JointAxisInternal[0] = 0.0;

  iobj_6->JointInternal.JointAxisInternal[1] = 0.0;

  iobj_6->JointInternal.JointAxisInternal[2] = 0.0;

  obj->Bodies[7] = iobj_6;

  obj->NumBodies = 8.0;

  obj->PositionNumber = 6.0;

  obj_0 = &obj->Base;

  b_kstr = obj->Base.NameInternal->size[0] * obj->Base.NameInternal->size[1];

  obj->Base.NameInternal->size[0] = 1;

  obj->Base.NameInternal->size[1] = 5;

  cartes_emxEnsureCapacity_char_T(obj->Base.NameInternal, b_kstr);

  for (b_kstr = 0; b_kstr < 5; b_kstr++) {

    obj->Base.NameInternal->data[b_kstr] = tmp_8[b_kstr];

  }



  b_kstr = obj->Base.JointInternal.Type->size[0] * obj->

    Base.JointInternal.Type->size[1];

  obj->Base.JointInternal.Type->size[0] = 1;

  obj->Base.JointInternal.Type->size[1] = 5;

  cartes_emxEnsureCapacity_char_T(obj->Base.JointInternal.Type, b_kstr);

  for (b_kstr = 0; b_kstr < 5; b_kstr++) {

    obj_0->JointInternal.Type->data[b_kstr] = tmp_7[b_kstr];

  }



  b_kstr = switch_expression->size[0] * switch_expression->size[1];

  switch_expression->size[0] = 1;

  switch_expression->size[1] = obj->Base.JointInternal.Type->size[1];

  cartes_emxEnsureCapacity_char_T(switch_expression, b_kstr);

  loop_ub = obj->Base.JointInternal.Type->size[0] * obj->

    Base.JointInternal.Type->size[1] - 1;

  for (b_kstr = 0; b_kstr <= loop_ub; b_kstr++) {

    switch_expression->data[b_kstr] = obj_0->JointInternal.Type->data[b_kstr];

  }



  b_bool = false;

  if (switch_expression->size[1] == 8) {

    b_kstr = 1;

    do {

      exitg1 = 0;

      if (b_kstr - 1 < 8) {

        loop_ub = b_kstr - 1;

        if (switch_expression->data[loop_ub] != b[loop_ub]) {

          exitg1 = 1;

        } else {

          b_kstr++;

        }

      } else {

        b_bool = true;

        exitg1 = 1;

      }

    } while (exitg1 == 0);

  }



  if (b_bool) {

    b_kstr = 0;

  } else {

    for (b_kstr = 0; b_kstr < 9; b_kstr++) {

      b_0[b_kstr] = tmp_1[b_kstr];

    }



    b_bool = false;

    if (switch_expression->size[1] == 9) {

      b_kstr = 1;

      do {

        exitg1 = 0;

        if (b_kstr - 1 < 9) {

          loop_ub = b_kstr - 1;

          if (switch_expression->data[loop_ub] != b_0[loop_ub]) {

            exitg1 = 1;

          } else {

            b_kstr++;

          }

        } else {

          b_bool = true;

          exitg1 = 1;

        }

      } while (exitg1 == 0);

    }



    if (b_bool) {

      b_kstr = 1;

    } else {

      b_kstr = -1;

    }

  }



  cartesian_waypoi_emxFree_char_T(&switch_expression);

  switch (b_kstr) {

   case 0:

    obj->Base.JointInternal.PositionNumber = 1.0;

    obj->Base.JointInternal.JointAxisInternal[0] = 0.0;

    obj->Base.JointInternal.JointAxisInternal[1] = 0.0;

    obj->Base.JointInternal.JointAxisInternal[2] = 1.0;

    break;



   case 1:

    obj->Base.JointInternal.PositionNumber = 1.0;

    obj->Base.JointInternal.JointAxisInternal[0] = 0.0;

    obj->Base.JointInternal.JointAxisInternal[1] = 0.0;

    obj->Base.JointInternal.JointAxisInternal[2] = 1.0;

    break;



   default:

    obj->Base.JointInternal.PositionNumber = 0.0;

    obj->Base.JointInternal.JointAxisInternal[0] = 0.0;

    obj->Base.JointInternal.JointAxisInternal[1] = 0.0;

    obj->Base.JointInternal.JointAxisInternal[2] = 0.0;

    break;

  }



  return b_obj;

}



// Model step function

void cartesian_waypoints_planner_step(void)

{

  robotics_slmanip_internal_blo_T *obj;

  robotics_slmanip_internal_b_e_T *obj_0;

  p_robotics_manip_internal_Rig_T *obj_1;

  emxArray_f_cell_wrap_cartesia_T *Ttree;

  emxArray_char_T_cartesian_way_T *bname;

  n_robotics_manip_internal_Rig_T *obj_2;

  static const char_T tmp[11] = { 'e', 'd', 'o', '_', 'l', 'i', 'n', 'k', '_',

    'e', 'e' };



  static const char_T tmp_0[5] = { 'w', 'o', 'r', 'l', 'd' };



  boolean_T exitg1;

  int32_T exitg2;



  // MATLABSystem: '<S11>/Get Parameter'

  ParamGet_cartesian_waypoints_planner_346.get_parameter

    (&cartesian_waypoints_planner_B.value_h);



  // MATLABSystem: '<S19>/Get Parameter'

  ParamGet_cartesian_waypoints_planner_288.get_parameter

    (&cartesian_waypoints_planner_B.c_n);



  // MATLABSystem: '<S19>/Get Parameter1'

  ParamGet_cartesian_waypoints_planner_289.get_parameter

    (&cartesian_waypoints_planner_B.value);



  // MATLABSystem: '<S19>/Get Parameter2'

  ParamGet_cartesian_waypoints_planner_290.get_parameter

    (&cartesian_waypoints_planner_B.value_c);



  // MATLABSystem: '<S19>/Get Parameter3'

  ParamGet_cartesian_waypoints_planner_291.get_parameter

    (&cartesian_waypoints_planner_B.value_cj);



  // MATLABSystem: '<S19>/Get Parameter4'

  ParamGet_cartesian_waypoints_planner_292.get_parameter

    (&cartesian_waypoints_planner_B.value_m);



  // MATLABSystem: '<S19>/Get Parameter5'

  ParamGet_cartesian_waypoints_planner_293.get_parameter

    (&cartesian_waypoints_planner_B.value_j);



  // MATLABSystem: '<S19>/Get Parameter6'

  ParamGet_cartesian_waypoints_planner_294.get_parameter

    (&cartesian_waypoints_planner_B.value_k);



  // MATLABSystem: '<S20>/Get Parameter'

  ParamGet_cartesian_waypoints_planner_300.get_parameter

    (&cartesian_waypoints_planner_B.value_mx);



  // MATLABSystem: '<S20>/Get Parameter1'

  ParamGet_cartesian_waypoints_planner_301.get_parameter

    (&cartesian_waypoints_planner_B.value_p);



  // MATLABSystem: '<S20>/Get Parameter2'

  ParamGet_cartesian_waypoints_planner_302.get_parameter

    (&cartesian_waypoints_planner_B.value_d);



  // MATLABSystem: '<S20>/Get Parameter3'

  ParamGet_cartesian_waypoints_planner_303.get_parameter

    (&cartesian_waypoints_planner_B.theta0);



  // MATLABSystem: '<S20>/Get Parameter4'

  ParamGet_cartesian_waypoints_planner_304.get_parameter

    (&cartesian_waypoints_planner_B.value_g);



  // MATLABSystem: '<S20>/Get Parameter5'

  ParamGet_cartesian_waypoints_planner_305.get_parameter

    (&cartesian_waypoints_planner_B.dp);



  // MATLABSystem: '<S20>/Get Parameter6'

  ParamGet_cartesian_waypoints_planner_306.get_parameter

    (&cartesian_waypoints_planner_B.d_n);



  // MATLAB Function: '<Root>/MATLAB Function' incorporates:

  //   Delay: '<Root>/Delay3'

  //   MATLABSystem: '<S11>/Get Parameter'

  //   MATLABSystem: '<S19>/Get Parameter'

  //   MATLABSystem: '<S19>/Get Parameter1'

  //   MATLABSystem: '<S19>/Get Parameter2'

  //   MATLABSystem: '<S19>/Get Parameter3'

  //   MATLABSystem: '<S19>/Get Parameter4'

  //   MATLABSystem: '<S19>/Get Parameter5'

  //   MATLABSystem: '<S19>/Get Parameter6'

  //   MATLABSystem: '<S20>/Get Parameter'

  //   MATLABSystem: '<S20>/Get Parameter1'

  //   MATLABSystem: '<S20>/Get Parameter2'

  //   MATLABSystem: '<S20>/Get Parameter3'

  //   MATLABSystem: '<S20>/Get Parameter4'

  //   MATLABSystem: '<S20>/Get Parameter5'

  //   MATLABSystem: '<S20>/Get Parameter6'



  if (cartesian_waypoints_planner_B.value_h > 20) {

    cartesian_waypoints_planner_B.value_h = 20;

  }



  cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c = 0.0;

  cartesian_waypoints_planner_B.b_n_g = 0.0;

  cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx = 0.0;

  cartesian_waypoints_planner_B.K24 = 0.0;

  cartesian_waypoints_planner_B.vec[0] = 0.0;

  cartesian_waypoints_planner_B.vec[1] = 0.0;

  cartesian_waypoints_planner_B.vec[2] = 0.0;

  if (cartesian_waypoints_planner_DW.Delay3_DSTATE <= 1.0) {

    cartesian_waypoints_planner_B.vec[0] = cartesian_waypoints_planner_B.c_n;

    cartesian_waypoints_planner_B.vec[1] = cartesian_waypoints_planner_B.value;

    cartesian_waypoints_planner_B.vec[2] = cartesian_waypoints_planner_B.value_c;

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c =

      cartesian_waypoints_planner_B.value_cj;

    cartesian_waypoints_planner_B.b_n_g = cartesian_waypoints_planner_B.value_m;

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx =

      cartesian_waypoints_planner_B.value_j;

    cartesian_waypoints_planner_B.K24 = cartesian_waypoints_planner_B.value_k;

  } else if (cartesian_waypoints_planner_DW.Delay3_DSTATE <

             cartesian_waypoints_planner_B.value_h) {

    cartesian_waypoints_planner_B.vec[0] =

      (cartesian_waypoints_planner_B.value_mx -

       cartesian_waypoints_planner_B.c_n) / static_cast<real_T>

      (cartesian_waypoints_planner_B.value_h) *

      cartesian_waypoints_planner_DW.Delay3_DSTATE +

      cartesian_waypoints_planner_B.c_n;

    cartesian_waypoints_planner_B.vec[1] =

      (cartesian_waypoints_planner_B.value_p -

       cartesian_waypoints_planner_B.value) / static_cast<real_T>

      (cartesian_waypoints_planner_B.value_h) *

      cartesian_waypoints_planner_DW.Delay3_DSTATE +

      cartesian_waypoints_planner_B.value;

    cartesian_waypoints_planner_B.vec[2] =

      (cartesian_waypoints_planner_B.value_d -

       cartesian_waypoints_planner_B.value_c) / static_cast<real_T>

      (cartesian_waypoints_planner_B.value_h) *

      cartesian_waypoints_planner_DW.Delay3_DSTATE +

      cartesian_waypoints_planner_B.value_c;

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c =

      cartesian_waypoints_planner_DW.Delay3_DSTATE / static_cast<real_T>

      (cartesian_waypoints_planner_B.value_h);

    cartesian_waypoints_planner_B.b_n_g = sqrt

      (((cartesian_waypoints_planner_B.value_cj *

         cartesian_waypoints_planner_B.value_cj +

         cartesian_waypoints_planner_B.value_m *

         cartesian_waypoints_planner_B.value_m) +

        cartesian_waypoints_planner_B.value_j *

        cartesian_waypoints_planner_B.value_j) +

       cartesian_waypoints_planner_B.value_k *

       cartesian_waypoints_planner_B.value_k);

    cartesian_waypoints_planner_B.value_cj /=

      cartesian_waypoints_planner_B.b_n_g;

    cartesian_waypoints_planner_B.value_m /= cartesian_waypoints_planner_B.b_n_g;

    cartesian_waypoints_planner_B.value_j /= cartesian_waypoints_planner_B.b_n_g;

    cartesian_waypoints_planner_B.value_k /= cartesian_waypoints_planner_B.b_n_g;

    cartesian_waypoints_planner_B.c_n = sqrt

      (((cartesian_waypoints_planner_B.theta0 *

         cartesian_waypoints_planner_B.theta0 +

         cartesian_waypoints_planner_B.value_g *

         cartesian_waypoints_planner_B.value_g) +

        cartesian_waypoints_planner_B.dp * cartesian_waypoints_planner_B.dp) +

       cartesian_waypoints_planner_B.d_n * cartesian_waypoints_planner_B.d_n);

    cartesian_waypoints_planner_B.b_n_g = cartesian_waypoints_planner_B.theta0 /

      cartesian_waypoints_planner_B.c_n;

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx =

      cartesian_waypoints_planner_B.value_g / cartesian_waypoints_planner_B.c_n;

    cartesian_waypoints_planner_B.K24 = cartesian_waypoints_planner_B.dp /

      cartesian_waypoints_planner_B.c_n;

    cartesian_waypoints_planner_B.d_n /= cartesian_waypoints_planner_B.c_n;

    cartesian_waypoints_planner_B.dp = ((cartesian_waypoints_planner_B.value_cj *

      cartesian_waypoints_planner_B.b_n_g +

      cartesian_waypoints_planner_B.value_m *

      cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx) +

      cartesian_waypoints_planner_B.value_j * cartesian_waypoints_planner_B.K24)

      + cartesian_waypoints_planner_B.value_k *

      cartesian_waypoints_planner_B.d_n;

    if (cartesian_waypoints_planner_B.dp < 0.0) {

      car_quaternioncg_parenReference(cartesian_waypoints_planner_B.b_n_g,

        cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx,

        cartesian_waypoints_planner_B.K24, cartesian_waypoints_planner_B.d_n,

        &cartesian_waypoints_planner_B.b_n_g,

        &cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx,

        &cartesian_waypoints_planner_B.K24, &cartesian_waypoints_planner_B.d_n);

      cartesian_waypoints_planner_B.b_n_g = -cartesian_waypoints_planner_B.b_n_g;

      cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx =

        -cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx;

      cartesian_waypoints_planner_B.K24 = -cartesian_waypoints_planner_B.K24;

      cartesian_waypoints_planner_B.d_n = -cartesian_waypoints_planner_B.d_n;

      cartesian_waypoints_planner_B.dp = -cartesian_waypoints_planner_B.dp;

    }



    if (cartesian_waypoints_planner_B.dp > 1.0) {

      cartesian_waypoints_planner_B.dp = 1.0;

    }



    cartesian_waypoints_planner_B.theta0 = acos(cartesian_waypoints_planner_B.dp);

    cartesian_waypoints_planner_B.dp = 1.0 / sin

      (cartesian_waypoints_planner_B.theta0);

    cartesian_waypoints_planner_B.value_g = sin((1.0 -

      cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c) *

      cartesian_waypoints_planner_B.theta0);

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c = sin

      (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c *

       cartesian_waypoints_planner_B.theta0);

    cartesian_waypoints_planner_B.b_n_g *=

      cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c;

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx *=

      cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c;

    cartesian_waypoints_planner_B.K24 *=

      cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c;

    cartesian_waypoints_planner_B.d_n *=

      cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c;

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c =

      (cartesian_waypoints_planner_B.value_g *

       cartesian_waypoints_planner_B.value_cj +

       cartesian_waypoints_planner_B.b_n_g) * cartesian_waypoints_planner_B.dp;

    cartesian_waypoints_planner_B.b_n_g = (cartesian_waypoints_planner_B.value_g

      * cartesian_waypoints_planner_B.value_m +

      cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx) *

      cartesian_waypoints_planner_B.dp;

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx =

      (cartesian_waypoints_planner_B.value_g *

       cartesian_waypoints_planner_B.value_j + cartesian_waypoints_planner_B.K24)

      * cartesian_waypoints_planner_B.dp;

    cartesian_waypoints_planner_B.K24 = (cartesian_waypoints_planner_B.value_g *

      cartesian_waypoints_planner_B.value_k + cartesian_waypoints_planner_B.d_n)

      * cartesian_waypoints_planner_B.dp;

    if (rtIsInf(cartesian_waypoints_planner_B.dp)) {

      car_quaternioncg_parenReference(cartesian_waypoints_planner_B.value_cj,

        cartesian_waypoints_planner_B.value_m,

        cartesian_waypoints_planner_B.value_j,

        cartesian_waypoints_planner_B.value_k,

        &cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c,

        &cartesian_waypoints_planner_B.b_n_g,

        &cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx,

        &cartesian_waypoints_planner_B.K24);

    }



    cartesian_waypoints_planner_B.d_n = sqrt

      (((cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c *

         cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c +

         cartesian_waypoints_planner_B.b_n_g *

         cartesian_waypoints_planner_B.b_n_g) +

        cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx *

        cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx) +

       cartesian_waypoints_planner_B.K24 * cartesian_waypoints_planner_B.K24);

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c /=

      cartesian_waypoints_planner_B.d_n;

    cartesian_waypoints_planner_B.b_n_g /= cartesian_waypoints_planner_B.d_n;

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx /=

      cartesian_waypoints_planner_B.d_n;

    cartesian_waypoints_planner_B.K24 /= cartesian_waypoints_planner_B.d_n;

  } else {

    if (cartesian_waypoints_planner_DW.Delay3_DSTATE >=

        cartesian_waypoints_planner_B.value_h) {

      cartesian_waypoints_planner_B.vec[0] =

        cartesian_waypoints_planner_B.value_mx;

      cartesian_waypoints_planner_B.vec[1] =

        cartesian_waypoints_planner_B.value_p;

      cartesian_waypoints_planner_B.vec[2] =

        cartesian_waypoints_planner_B.value_d;

      cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c =

        cartesian_waypoints_planner_B.theta0;

      cartesian_waypoints_planner_B.b_n_g =

        cartesian_waypoints_planner_B.value_g;

      cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx =

        cartesian_waypoints_planner_B.dp;

      cartesian_waypoints_planner_B.K24 = cartesian_waypoints_planner_B.d_n;

    }

  }



  cartesian_waypoints_planner_DW.Delay3_DSTATE++;



  // MATLABSystem: '<Root>/Coordinate Transformation Conversion3' incorporates:

  //   MATLAB Function: '<Root>/MATLAB Function'



  cartesian_waypoints_planner_B.d_n = 1.0 / sqrt

    (((cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c *

       cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c +

       cartesian_waypoints_planner_B.b_n_g * cartesian_waypoints_planner_B.b_n_g)

      + cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx *

      cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx) +

     cartesian_waypoints_planner_B.K24 * cartesian_waypoints_planner_B.K24);

  cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c *=

    cartesian_waypoints_planner_B.d_n;

  cartesian_waypoints_planner_B.b_n_g *= cartesian_waypoints_planner_B.d_n;

  cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx *=

    cartesian_waypoints_planner_B.d_n;

  cartesian_waypoints_planner_B.d_n *= cartesian_waypoints_planner_B.K24;

  cartesian_waypoints_planner_B.K24 = cartesian_waypoints_planner_B.d_n *

    cartesian_waypoints_planner_B.d_n;

  cartesian_waypoints_planner_B.dp =

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx *

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx;

  cartesian_waypoints_planner_B.tempR[0] = 1.0 -

    (cartesian_waypoints_planner_B.dp + cartesian_waypoints_planner_B.K24) * 2.0;

  cartesian_waypoints_planner_B.value_cj = cartesian_waypoints_planner_B.b_n_g *

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx;

  cartesian_waypoints_planner_B.value_g =

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c *

    cartesian_waypoints_planner_B.d_n;

  cartesian_waypoints_planner_B.tempR[1] =

    (cartesian_waypoints_planner_B.value_cj -

     cartesian_waypoints_planner_B.value_g) * 2.0;

  cartesian_waypoints_planner_B.value_m = cartesian_waypoints_planner_B.b_n_g *

    cartesian_waypoints_planner_B.d_n;

  cartesian_waypoints_planner_B.theta0 =

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c *

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx;

  cartesian_waypoints_planner_B.tempR[2] =

    (cartesian_waypoints_planner_B.value_m +

     cartesian_waypoints_planner_B.theta0) * 2.0;

  cartesian_waypoints_planner_B.tempR[3] =

    (cartesian_waypoints_planner_B.value_cj +

     cartesian_waypoints_planner_B.value_g) * 2.0;

  cartesian_waypoints_planner_B.value_cj = cartesian_waypoints_planner_B.b_n_g *

    cartesian_waypoints_planner_B.b_n_g;

  cartesian_waypoints_planner_B.tempR[4] = 1.0 -

    (cartesian_waypoints_planner_B.value_cj + cartesian_waypoints_planner_B.K24)

    * 2.0;

  cartesian_waypoints_planner_B.K24 =

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx *

    cartesian_waypoints_planner_B.d_n;

  cartesian_waypoints_planner_B.value_g =

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c *

    cartesian_waypoints_planner_B.b_n_g;

  cartesian_waypoints_planner_B.tempR[5] = (cartesian_waypoints_planner_B.K24 -

    cartesian_waypoints_planner_B.value_g) * 2.0;

  cartesian_waypoints_planner_B.tempR[6] =

    (cartesian_waypoints_planner_B.value_m -

     cartesian_waypoints_planner_B.theta0) * 2.0;

  cartesian_waypoints_planner_B.tempR[7] = (cartesian_waypoints_planner_B.K24 +

    cartesian_waypoints_planner_B.value_g) * 2.0;

  cartesian_waypoints_planner_B.tempR[8] = 1.0 -

    (cartesian_waypoints_planner_B.value_cj + cartesian_waypoints_planner_B.dp) *

    2.0;

  for (cartesian_waypoints_planner_B.value_h = 0;

       cartesian_waypoints_planner_B.value_h < 3;

       cartesian_waypoints_planner_B.value_h++) {

    cartesian_waypoints_planner_B.subsa_idx_1 =

      cartesian_waypoints_planner_B.value_h + 1;

    cartesian_waypoints_planner_B.R_a[cartesian_waypoints_planner_B.subsa_idx_1

      - 1] = cartesian_waypoints_planner_B.tempR

      [(cartesian_waypoints_planner_B.subsa_idx_1 - 1) * 3];

    cartesian_waypoints_planner_B.subsa_idx_1 =

      cartesian_waypoints_planner_B.value_h + 1;

    cartesian_waypoints_planner_B.R_a[cartesian_waypoints_planner_B.subsa_idx_1

      + 2] = cartesian_waypoints_planner_B.tempR

      [(cartesian_waypoints_planner_B.subsa_idx_1 - 1) * 3 + 1];

    cartesian_waypoints_planner_B.subsa_idx_1 =

      cartesian_waypoints_planner_B.value_h + 1;

    cartesian_waypoints_planner_B.R_a[cartesian_waypoints_planner_B.subsa_idx_1

      + 5] = cartesian_waypoints_planner_B.tempR

      [(cartesian_waypoints_planner_B.subsa_idx_1 - 1) * 3 + 2];

  }



  memset(&cartesian_waypoints_planner_B.out[0], 0, sizeof(real_T) << 4U);

  cartesian_waypoints_planner_B.out[15] = 1.0;

  for (cartesian_waypoints_planner_B.value_h = 0;

       cartesian_waypoints_planner_B.value_h < 3;

       cartesian_waypoints_planner_B.value_h++) {

    cartesian_waypoints_planner_B.subsa_idx_1 =

      cartesian_waypoints_planner_B.value_h << 2;

    cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.subsa_idx_1]

      = cartesian_waypoints_planner_B.R_a[3 *

      cartesian_waypoints_planner_B.value_h];

    cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.subsa_idx_1

      + 1] = cartesian_waypoints_planner_B.R_a[3 *

      cartesian_waypoints_planner_B.value_h + 1];

    cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.subsa_idx_1

      + 2] = cartesian_waypoints_planner_B.R_a[3 *

      cartesian_waypoints_planner_B.value_h + 2];

    cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.value_h + 12]

      = cartesian_waypoints_planner_B.vec[cartesian_waypoints_planner_B.value_h];

  }



  // MATLABSystem: '<S2>/MATLAB System' incorporates:

  //   Delay: '<Root>/Delay'

  //   MATLABSystem: '<Root>/Coordinate Transformation Conversion3'

  //   MATLABSystem: '<S12>/Get Parameter'

  //   MATLABSystem: '<S12>/Get Parameter1'

  //   MATLABSystem: '<S12>/Get Parameter2'

  //   MATLABSystem: '<S12>/Get Parameter3'

  //   MATLABSystem: '<S12>/Get Parameter4'

  //   MATLABSystem: '<S12>/Get Parameter5'



  ParamGet_cartesian_waypoints_planner_312.get_parameter

    (&cartesian_waypoints_planner_B.max_vel[0]);

  ParamGet_cartesian_waypoints_planner_313.get_parameter

    (&cartesian_waypoints_planner_B.max_vel[1]);

  ParamGet_cartesian_waypoints_planner_314.get_parameter

    (&cartesian_waypoints_planner_B.max_vel[2]);

  ParamGet_cartesian_waypoints_planner_315.get_parameter

    (&cartesian_waypoints_planner_B.max_vel[3]);

  ParamGet_cartesian_waypoints_planner_316.get_parameter

    (&cartesian_waypoints_planner_B.max_vel[4]);

  ParamGet_cartesian_waypoints_planner_317.get_parameter

    (&cartesian_waypoints_planner_B.max_vel[5]);

  obj = &cartesian_waypoints_planner_DW.obj;

  if (cartesian_waypoints_planner_DW.obj.IKInternal.isInitialized != 1) {

    cartesian_waypoints_planner_DW.obj.IKInternal.isSetupComplete = false;

    cartesian_waypoints_planner_DW.obj.IKInternal.isInitialized = 1;

    car_inverseKinematics_setupImpl

      (&cartesian_waypoints_planner_DW.obj.IKInternal,

       &cartesian_waypoints_planner_DW.gobj_85);

    obj->IKInternal.isSetupComplete = true;

  }



  cart_inverseKinematics_stepImpl(&obj->IKInternal,

    cartesian_waypoints_planner_B.out, cartesian_waypoints_planner_B.max_vel,

    cartesian_waypoints_planner_DW.Delay_DSTATE,

    cartesian_waypoints_planner_B.b_varargout_1);



  // MATLAB Function: '<Root>/MATLAB Function2' incorporates:

  //   Delay: '<Root>/Delay1'

  //   Delay: '<Root>/Delay2'

  //   MATLABSystem: '<S2>/MATLAB System'



  if ((cartesian_waypoints_planner_DW.Delay1_DSTATE >= 1.0) &&

      (cartesian_waypoints_planner_DW.Delay1_DSTATE <= 20.0)) {

    cartesian_waypoints_planner_B.value_h = static_cast<int32_T>

      (cartesian_waypoints_planner_DW.Delay1_DSTATE);

    for (cartesian_waypoints_planner_B.subsa_idx_1 = 0;

         cartesian_waypoints_planner_B.subsa_idx_1 < 6;

         cartesian_waypoints_planner_B.subsa_idx_1++) {

      cartesian_waypoints_planner_DW.Delay2_DSTATE[cartesian_waypoints_planner_B.subsa_idx_1

        + 6 * (cartesian_waypoints_planner_B.value_h - 1)] =

        cartesian_waypoints_planner_B.b_varargout_1[cartesian_waypoints_planner_B.subsa_idx_1];

    }

  }



  cartesian_waypoints_planner_DW.Delay1_DSTATE++;



  // Outputs for Atomic SubSystem: '<Root>/Subscribe1'

  // MATLABSystem: '<S10>/SourceBlock' incorporates:

  //   Inport: '<S18>/In1'



  cartesian_waypoints_planner_B.b_varargout_1_j =

    Sub_cartesian_waypoints_planner_384.getLatestMessage

    (&cartesian_waypoints_planner_B.b_varargout_2);



  // Outputs for Enabled SubSystem: '<S10>/Enabled Subsystem' incorporates:

  //   EnablePort: '<S18>/Enable'



  if (cartesian_waypoints_planner_B.b_varargout_1_j) {

    cartesian_waypoints_planner_B.In1 =

      cartesian_waypoints_planner_B.b_varargout_2;

  }



  // End of MATLABSystem: '<S10>/SourceBlock'

  // End of Outputs for SubSystem: '<S10>/Enabled Subsystem'

  // End of Outputs for SubSystem: '<Root>/Subscribe1'



  // MATLABSystem: '<Root>/Get Parameter'

  ParamGet_cartesian_waypoints_planner_378.get_parameter

    (&cartesian_waypoints_planner_B.c_n);



  // MATLAB Function: '<Root>/MATLAB Function3' incorporates:

  //   MATLABSystem: '<Root>/Get Parameter'



  cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c =

    (cartesian_waypoints_planner_B.In1.Clock_.Nsec / 1.0E+9 +

     cartesian_waypoints_planner_B.In1.Clock_.Sec) -

    cartesian_waypoints_planner_B.c_n;

  if (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c < 0.0) {

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c = 0.0;

  }



  // End of MATLAB Function: '<Root>/MATLAB Function3'



  // MATLABSystem: '<Root>/Get Parameter1'

  ParamGet_cartesian_waypoints_planner_379.get_parameter

    (&cartesian_waypoints_planner_B.c_n);



  // MATLABSystem: '<Root>/Get Parameter2'

  ParamGet_cartesian_waypoints_planner_380.get_parameter

    (&cartesian_waypoints_planner_B.value);



  // MATLAB Function: '<Root>/MATLAB Function4' incorporates:

  //   Delay: '<Root>/Delay2'

  //   MATLABSystem: '<Root>/Get Parameter1'

  //   MATLABSystem: '<Root>/Get Parameter2'



  for (cartesian_waypoints_planner_B.i = 0; cartesian_waypoints_planner_B.i < 6;

       cartesian_waypoints_planner_B.i++) {

    cartesian_waypoints_planner_B.max_vel[cartesian_waypoints_planner_B.i] =

      cartesian_waypoints_planner_B.c_n;

    cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.i] =

      cartesian_waypoints_planner_B.value;

  }



  memset(&cartesian_waypoints_planner_B.signes[0], 0, 114U * sizeof(real_T));

  memset(&cartesian_waypoints_planner_B.t[0], 0, 342U * sizeof(real_T));

  for (cartesian_waypoints_planner_B.value_h = 0;

       cartesian_waypoints_planner_B.value_h < 36;

       cartesian_waypoints_planner_B.value_h++) {

    cartesian_waypoints_planner_B.d_t[cartesian_waypoints_planner_B.value_h] =

      0.0;

    cartesian_waypoints_planner_B.steps[cartesian_waypoints_planner_B.value_h] =

      1.0;

  }



  for (cartesian_waypoints_planner_B.i = 0; cartesian_waypoints_planner_B.i < 6;

       cartesian_waypoints_planner_B.i++) {

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx =

      cartesian_waypoints_planner_B.max_vel[cartesian_waypoints_planner_B.i] /

      cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.i];

    cartesian_waypoints_planner_B.b_n_g =

      cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c;

    cartesian_waypoints_planner_B.j = 0;

    exitg1 = false;

    while ((!exitg1) && (cartesian_waypoints_planner_B.j < 19)) {

      cartesian_waypoints_planner_B.dist_tmp = 6 *

        cartesian_waypoints_planner_B.j + cartesian_waypoints_planner_B.i;

      cartesian_waypoints_planner_B.d_n =

        cartesian_waypoints_planner_DW.Delay2_DSTATE

        [(cartesian_waypoints_planner_B.j + 1) * 6 +

        cartesian_waypoints_planner_B.i] -

        cartesian_waypoints_planner_DW.Delay2_DSTATE[cartesian_waypoints_planner_B.dist_tmp];

      if (cartesian_waypoints_planner_B.d_n < 0.0) {

        cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.dist_tmp]

          = -1.0;

      } else if (cartesian_waypoints_planner_B.d_n > 0.0) {

        cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.dist_tmp]

          = 1.0;

      } else if (cartesian_waypoints_planner_B.d_n == 0.0) {

        cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.dist_tmp]

          = 0.0;

      } else {

        cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.dist_tmp]

          = (rtNaN);

      }



      cartesian_waypoints_planner_B.d_n = fabs(cartesian_waypoints_planner_B.d_n);

      cartesian_waypoints_planner_B.K24 =

        cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx *

        cartesian_waypoints_planner_B.max_vel[cartesian_waypoints_planner_B.i];

      if (cartesian_waypoints_planner_B.d_n < cartesian_waypoints_planner_B.K24)

      {

        cartesian_waypoints_planner_B.d_n = sqrt

          (cartesian_waypoints_planner_B.d_n /

           cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.i]);

        cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.dist_tmp +

          114] = 0.0;

        cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.dist_tmp] =

          cartesian_waypoints_planner_B.d_n;

        cartesian_waypoints_planner_B.t[(cartesian_waypoints_planner_B.i + 6 *

          cartesian_waypoints_planner_B.j) + 228] =

          cartesian_waypoints_planner_B.d_n;

      } else {

        cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.dist_tmp] =

          cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx;

        cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.dist_tmp +

          228] = cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx;

        cartesian_waypoints_planner_B.t[(cartesian_waypoints_planner_B.i + 6 *

          cartesian_waypoints_planner_B.j) + 114] =

          (cartesian_waypoints_planner_B.d_n - cartesian_waypoints_planner_B.K24)

          /

          cartesian_waypoints_planner_B.max_vel[cartesian_waypoints_planner_B.i];

      }



      cartesian_waypoints_planner_B.value_h = (6 *

        cartesian_waypoints_planner_B.j + cartesian_waypoints_planner_B.i) + 114;

      cartesian_waypoints_planner_B.subsa_idx_1 = (6 *

        cartesian_waypoints_planner_B.j + cartesian_waypoints_planner_B.i) + 228;

      cartesian_waypoints_planner_B.b_n_g =

        ((cartesian_waypoints_planner_B.b_n_g -

          cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.dist_tmp])

         - cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.value_h])

        - cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.subsa_idx_1];

      if (cartesian_waypoints_planner_B.b_n_g >= 0.0) {

        cartesian_waypoints_planner_B.steps[cartesian_waypoints_planner_B.i]++;

        cartesian_waypoints_planner_B.d_t[cartesian_waypoints_planner_B.i] =

          ((cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.dist_tmp]

            + cartesian_waypoints_planner_B.d_t[cartesian_waypoints_planner_B.i])

           + cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.value_h])

          + cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.subsa_idx_1];

        cartesian_waypoints_planner_B.j++;

      } else {

        exitg1 = true;

      }

    }

  }



  for (cartesian_waypoints_planner_B.i = 0; cartesian_waypoints_planner_B.i < 6;

       cartesian_waypoints_planner_B.i++) {

    cartesian_waypoints_planner_B.b_n_g =

      cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c -

      cartesian_waypoints_planner_B.d_t[cartesian_waypoints_planner_B.i];

    cartesian_waypoints_planner_B.j = (static_cast<int32_T>

      (cartesian_waypoints_planner_B.steps[cartesian_waypoints_planner_B.i]) - 1)

      * 6 + cartesian_waypoints_planner_B.i;

    if (cartesian_waypoints_planner_B.steps[cartesian_waypoints_planner_B.i] ==

        20.0) {

      cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.i] =

        cartesian_waypoints_planner_DW.Delay2_DSTATE[cartesian_waypoints_planner_B.i

        + 114];

      cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.i] = 0.0;

      cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.i] = 0.0;

    } else if (cartesian_waypoints_planner_B.b_n_g <= 0.0) {

      cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.i] =

        cartesian_waypoints_planner_DW.Delay2_DSTATE[cartesian_waypoints_planner_B.j];

      cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.i] = 0.0;

      cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.i] = 0.0;

    } else {

      cartesian_waypoints_planner_B.d_n = cartesian_waypoints_planner_B.b_n_g -

        cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.j];

      if (cartesian_waypoints_planner_B.d_n < 0.0) {

        cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.i] =

          cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.j] *

          (0.5 *

           cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.i] *

           (cartesian_waypoints_planner_B.b_n_g *

            cartesian_waypoints_planner_B.b_n_g)) +

          cartesian_waypoints_planner_DW.Delay2_DSTATE[cartesian_waypoints_planner_B.j];

        cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.i] =

          cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.j] *

          (cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.i] *

           cartesian_waypoints_planner_B.b_n_g);

        cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.i] =

          cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.j] *

          cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.i];

      } else {

        cartesian_waypoints_planner_B.value_h = cartesian_waypoints_planner_B.j

          + 114;

        cartesian_waypoints_planner_B.d_n -=

          cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.value_h];

        if (cartesian_waypoints_planner_B.d_n < 0.0) {

          cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.i] =

            (cartesian_waypoints_planner_B.b_n_g -

             cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.j] /

             2.0) *

            cartesian_waypoints_planner_B.max_vel[cartesian_waypoints_planner_B.i]

            * cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.j]

            + cartesian_waypoints_planner_DW.Delay2_DSTATE[cartesian_waypoints_planner_B.j];

          cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.i] =

            cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.j]

            * cartesian_waypoints_planner_B.max_vel[cartesian_waypoints_planner_B.i];

          cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.i] =

            0.0;

        } else {

          cartesian_waypoints_planner_B.subsa_idx_1 =

            cartesian_waypoints_planner_B.j + 228;

          if (cartesian_waypoints_planner_B.d_n -

              cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.subsa_idx_1]

              < 0.0) {

            cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx =

              (cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.value_h]

               + cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.j])

              + cartesian_waypoints_planner_B.t[cartesian_waypoints_planner_B.subsa_idx_1];

            cartesian_waypoints_planner_B.d_n =

              cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx -

              cartesian_waypoints_planner_B.b_n_g;

            cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.i] =

              cartesian_waypoints_planner_DW.Delay2_DSTATE[(static_cast<int32_T>

              (cartesian_waypoints_planner_B.steps[cartesian_waypoints_planner_B.i]

               + 1.0) - 1) * 6 + cartesian_waypoints_planner_B.i] -

              cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.j]

              * (0.5 *

                 cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.i]

                 * (cartesian_waypoints_planner_B.d_n *

                    cartesian_waypoints_planner_B.d_n));

            cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.i] =

              (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx -

               cartesian_waypoints_planner_B.b_n_g) *

              cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.i]

              * cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.j];

            cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.i] =

              cartesian_waypoints_planner_B.signes[cartesian_waypoints_planner_B.j]

              * -cartesian_waypoints_planner_B.acc[cartesian_waypoints_planner_B.i];

          } else {

            cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.i] =

              cartesian_waypoints_planner_DW.Delay2_DSTATE[cartesian_waypoints_planner_B.j];

            cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.i] =

              0.0;

            cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.i] =

              0.0;

          }

        }

      }

    }

  }



  // End of MATLAB Function: '<Root>/MATLAB Function4'

  cartesian_w_emxInit_f_cell_wrap(&Ttree, 2);

  cartesian_waypoi_emxInit_char_T(&bname, 2);



  // MATLABSystem: '<S15>/MATLAB System'

  obj_0 = &cartesian_waypoints_planner_DW.obj_m;

  obj_1 = &cartesian_waypoints_planner_DW.obj_m.TreeInternal;

  RigidBodyTree_forwardKinematics(&obj_0->TreeInternal,

    cartesian_waypoints_planner_B.q, Ttree);

  cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c = -1.0;

  cartesian_waypoints_planner_B.value_h = bname->size[0] * bname->size[1];

  bname->size[0] = 1;

  bname->size[1] = obj_1->Base.NameInternal->size[1];

  cartes_emxEnsureCapacity_char_T(bname, cartesian_waypoints_planner_B.value_h);

  cartesian_waypoints_planner_B.subsa_idx_1 = obj_1->Base.NameInternal->size[0] *

    obj_1->Base.NameInternal->size[1] - 1;

  for (cartesian_waypoints_planner_B.value_h = 0;

       cartesian_waypoints_planner_B.value_h <=

       cartesian_waypoints_planner_B.subsa_idx_1;

       cartesian_waypoints_planner_B.value_h++) {

    bname->data[cartesian_waypoints_planner_B.value_h] =

      obj_1->Base.NameInternal->data[cartesian_waypoints_planner_B.value_h];

  }



  for (cartesian_waypoints_planner_B.value_h = 0;

       cartesian_waypoints_planner_B.value_h < 11;

       cartesian_waypoints_planner_B.value_h++) {

    cartesian_waypoints_planner_B.b_i[cartesian_waypoints_planner_B.value_h] =

      tmp[cartesian_waypoints_planner_B.value_h];

  }



  cartesian_waypoints_planner_B.b_varargout_1_j = false;

  if (bname->size[1] == 11) {

    cartesian_waypoints_planner_B.value_h = 1;

    do {

      exitg2 = 0;

      if (cartesian_waypoints_planner_B.value_h - 1 < 11) {

        cartesian_waypoints_planner_B.subsa_idx_1 =

          cartesian_waypoints_planner_B.value_h - 1;

        if (bname->data[cartesian_waypoints_planner_B.subsa_idx_1] !=

            cartesian_waypoints_planner_B.b_i[cartesian_waypoints_planner_B.subsa_idx_1])

        {

          exitg2 = 1;

        } else {

          cartesian_waypoints_planner_B.value_h++;

        }

      } else {

        cartesian_waypoints_planner_B.b_varargout_1_j = true;

        exitg2 = 1;

      }

    } while (exitg2 == 0);

  }



  if (cartesian_waypoints_planner_B.b_varargout_1_j) {

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c = 0.0;

  } else {

    cartesian_waypoints_planner_B.d_n = obj_0->TreeInternal.NumBodies;

    cartesian_waypoints_planner_B.i = 0;

    exitg1 = false;

    while ((!exitg1) && (cartesian_waypoints_planner_B.i <= static_cast<int32_T>

                         (cartesian_waypoints_planner_B.d_n) - 1)) {

      obj_2 = obj_1->Bodies[cartesian_waypoints_planner_B.i];

      cartesian_waypoints_planner_B.value_h = bname->size[0] * bname->size[1];

      bname->size[0] = 1;

      bname->size[1] = obj_2->NameInternal->size[1];

      cartes_emxEnsureCapacity_char_T(bname,

        cartesian_waypoints_planner_B.value_h);

      cartesian_waypoints_planner_B.subsa_idx_1 = obj_2->NameInternal->size[0] *

        obj_2->NameInternal->size[1] - 1;

      for (cartesian_waypoints_planner_B.value_h = 0;

           cartesian_waypoints_planner_B.value_h <=

           cartesian_waypoints_planner_B.subsa_idx_1;

           cartesian_waypoints_planner_B.value_h++) {

        bname->data[cartesian_waypoints_planner_B.value_h] = obj_2->

          NameInternal->data[cartesian_waypoints_planner_B.value_h];

      }



      for (cartesian_waypoints_planner_B.value_h = 0;

           cartesian_waypoints_planner_B.value_h < 11;

           cartesian_waypoints_planner_B.value_h++) {

        cartesian_waypoints_planner_B.b_i[cartesian_waypoints_planner_B.value_h]

          = tmp[cartesian_waypoints_planner_B.value_h];

      }



      cartesian_waypoints_planner_B.b_varargout_1_j = false;

      if (bname->size[1] == 11) {

        cartesian_waypoints_planner_B.value_h = 1;

        do {

          exitg2 = 0;

          if (cartesian_waypoints_planner_B.value_h - 1 < 11) {

            cartesian_waypoints_planner_B.subsa_idx_1 =

              cartesian_waypoints_planner_B.value_h - 1;

            if (bname->data[cartesian_waypoints_planner_B.subsa_idx_1] !=

                cartesian_waypoints_planner_B.b_i[cartesian_waypoints_planner_B.subsa_idx_1])

            {

              exitg2 = 1;

            } else {

              cartesian_waypoints_planner_B.value_h++;

            }

          } else {

            cartesian_waypoints_planner_B.b_varargout_1_j = true;

            exitg2 = 1;

          }

        } while (exitg2 == 0);

      }



      if (cartesian_waypoints_planner_B.b_varargout_1_j) {

        cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c =

          static_cast<real_T>(cartesian_waypoints_planner_B.i) + 1.0;

        exitg1 = true;

      } else {

        cartesian_waypoints_planner_B.i++;

      }

    }

  }



  if (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c == 0.0) {

    memset(&cartesian_waypoints_planner_B.out[0], 0, sizeof(real_T) << 4U);

    cartesian_waypoints_planner_B.out[0] = 1.0;

    cartesian_waypoints_planner_B.out[5] = 1.0;

    cartesian_waypoints_planner_B.out[10] = 1.0;

    cartesian_waypoints_planner_B.out[15] = 1.0;

  } else {

    for (cartesian_waypoints_planner_B.value_h = 0;

         cartesian_waypoints_planner_B.value_h < 16;

         cartesian_waypoints_planner_B.value_h++) {

      cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.value_h] =

        Ttree->data[static_cast<int32_T>

        (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c) - 1]

        .f1[cartesian_waypoints_planner_B.value_h];

    }

  }



  cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c = -1.0;

  cartesian_waypoints_planner_B.value_h = bname->size[0] * bname->size[1];

  bname->size[0] = 1;

  bname->size[1] = obj_1->Base.NameInternal->size[1];

  cartes_emxEnsureCapacity_char_T(bname, cartesian_waypoints_planner_B.value_h);

  cartesian_waypoints_planner_B.subsa_idx_1 = obj_1->Base.NameInternal->size[0] *

    obj_1->Base.NameInternal->size[1] - 1;

  for (cartesian_waypoints_planner_B.value_h = 0;

       cartesian_waypoints_planner_B.value_h <=

       cartesian_waypoints_planner_B.subsa_idx_1;

       cartesian_waypoints_planner_B.value_h++) {

    bname->data[cartesian_waypoints_planner_B.value_h] =

      obj_1->Base.NameInternal->data[cartesian_waypoints_planner_B.value_h];

  }



  for (cartesian_waypoints_planner_B.value_h = 0;

       cartesian_waypoints_planner_B.value_h < 5;

       cartesian_waypoints_planner_B.value_h++) {

    cartesian_waypoints_planner_B.b_jo[cartesian_waypoints_planner_B.value_h] =

      tmp_0[cartesian_waypoints_planner_B.value_h];

  }



  cartesian_waypoints_planner_B.b_varargout_1_j = false;

  if (bname->size[1] == 5) {

    cartesian_waypoints_planner_B.value_h = 1;

    do {

      exitg2 = 0;

      if (cartesian_waypoints_planner_B.value_h - 1 < 5) {

        cartesian_waypoints_planner_B.subsa_idx_1 =

          cartesian_waypoints_planner_B.value_h - 1;

        if (bname->data[cartesian_waypoints_planner_B.subsa_idx_1] !=

            cartesian_waypoints_planner_B.b_jo[cartesian_waypoints_planner_B.subsa_idx_1])

        {

          exitg2 = 1;

        } else {

          cartesian_waypoints_planner_B.value_h++;

        }

      } else {

        cartesian_waypoints_planner_B.b_varargout_1_j = true;

        exitg2 = 1;

      }

    } while (exitg2 == 0);

  }



  if (cartesian_waypoints_planner_B.b_varargout_1_j) {

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c = 0.0;

  } else {

    cartesian_waypoints_planner_B.d_n = obj_0->TreeInternal.NumBodies;

    cartesian_waypoints_planner_B.i = 0;

    exitg1 = false;

    while ((!exitg1) && (cartesian_waypoints_planner_B.i <= static_cast<int32_T>

                         (cartesian_waypoints_planner_B.d_n) - 1)) {

      obj_2 = obj_1->Bodies[cartesian_waypoints_planner_B.i];

      cartesian_waypoints_planner_B.value_h = bname->size[0] * bname->size[1];

      bname->size[0] = 1;

      bname->size[1] = obj_2->NameInternal->size[1];

      cartes_emxEnsureCapacity_char_T(bname,

        cartesian_waypoints_planner_B.value_h);

      cartesian_waypoints_planner_B.subsa_idx_1 = obj_2->NameInternal->size[0] *

        obj_2->NameInternal->size[1] - 1;

      for (cartesian_waypoints_planner_B.value_h = 0;

           cartesian_waypoints_planner_B.value_h <=

           cartesian_waypoints_planner_B.subsa_idx_1;

           cartesian_waypoints_planner_B.value_h++) {

        bname->data[cartesian_waypoints_planner_B.value_h] = obj_2->

          NameInternal->data[cartesian_waypoints_planner_B.value_h];

      }



      for (cartesian_waypoints_planner_B.value_h = 0;

           cartesian_waypoints_planner_B.value_h < 5;

           cartesian_waypoints_planner_B.value_h++) {

        cartesian_waypoints_planner_B.b_jo[cartesian_waypoints_planner_B.value_h]

          = tmp_0[cartesian_waypoints_planner_B.value_h];

      }



      cartesian_waypoints_planner_B.b_varargout_1_j = false;

      if (bname->size[1] == 5) {

        cartesian_waypoints_planner_B.value_h = 1;

        do {

          exitg2 = 0;

          if (cartesian_waypoints_planner_B.value_h - 1 < 5) {

            cartesian_waypoints_planner_B.subsa_idx_1 =

              cartesian_waypoints_planner_B.value_h - 1;

            if (bname->data[cartesian_waypoints_planner_B.subsa_idx_1] !=

                cartesian_waypoints_planner_B.b_jo[cartesian_waypoints_planner_B.subsa_idx_1])

            {

              exitg2 = 1;

            } else {

              cartesian_waypoints_planner_B.value_h++;

            }

          } else {

            cartesian_waypoints_planner_B.b_varargout_1_j = true;

            exitg2 = 1;

          }

        } while (exitg2 == 0);

      }



      if (cartesian_waypoints_planner_B.b_varargout_1_j) {

        cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c =

          static_cast<real_T>(cartesian_waypoints_planner_B.i) + 1.0;

        exitg1 = true;

      } else {

        cartesian_waypoints_planner_B.i++;

      }

    }

  }



  cartesian_waypoi_emxFree_char_T(&bname);



  // MATLABSystem: '<S15>/MATLAB System'

  if (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c == 0.0) {

    memset(&cartesian_waypoints_planner_B.T2[0], 0, sizeof(real_T) << 4U);

    cartesian_waypoints_planner_B.T2[0] = 1.0;

    cartesian_waypoints_planner_B.T2[5] = 1.0;

    cartesian_waypoints_planner_B.T2[10] = 1.0;

    cartesian_waypoints_planner_B.T2[15] = 1.0;

  } else {

    for (cartesian_waypoints_planner_B.value_h = 0;

         cartesian_waypoints_planner_B.value_h < 16;

         cartesian_waypoints_planner_B.value_h++) {

      cartesian_waypoints_planner_B.T2[cartesian_waypoints_planner_B.value_h] =

        Ttree->data[static_cast<int32_T>

        (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c) - 1]

        .f1[cartesian_waypoints_planner_B.value_h];

    }

  }



  cartesian_w_emxFree_f_cell_wrap(&Ttree);



  // MATLABSystem: '<S15>/MATLAB System'

  for (cartesian_waypoints_planner_B.value_h = 0;

       cartesian_waypoints_planner_B.value_h < 3;

       cartesian_waypoints_planner_B.value_h++) {

    cartesian_waypoints_planner_B.R_a[3 * cartesian_waypoints_planner_B.value_h]

      = cartesian_waypoints_planner_B.T2[cartesian_waypoints_planner_B.value_h];

    cartesian_waypoints_planner_B.R_a[3 * cartesian_waypoints_planner_B.value_h

      + 1] =

      cartesian_waypoints_planner_B.T2[cartesian_waypoints_planner_B.value_h + 4];

    cartesian_waypoints_planner_B.R_a[3 * cartesian_waypoints_planner_B.value_h

      + 2] =

      cartesian_waypoints_planner_B.T2[cartesian_waypoints_planner_B.value_h + 8];

  }



  for (cartesian_waypoints_planner_B.value_h = 0;

       cartesian_waypoints_planner_B.value_h < 9;

       cartesian_waypoints_planner_B.value_h++) {

    cartesian_waypoints_planner_B.tempR[cartesian_waypoints_planner_B.value_h] =

      -cartesian_waypoints_planner_B.R_a[cartesian_waypoints_planner_B.value_h];

  }



  for (cartesian_waypoints_planner_B.value_h = 0;

       cartesian_waypoints_planner_B.value_h < 3;

       cartesian_waypoints_planner_B.value_h++) {

    cartesian_waypoints_planner_B.subsa_idx_1 =

      cartesian_waypoints_planner_B.value_h << 2;

    cartesian_waypoints_planner_B.R[cartesian_waypoints_planner_B.subsa_idx_1] =

      cartesian_waypoints_planner_B.R_a[3 *

      cartesian_waypoints_planner_B.value_h];

    cartesian_waypoints_planner_B.R[cartesian_waypoints_planner_B.subsa_idx_1 +

      1] = cartesian_waypoints_planner_B.R_a[3 *

      cartesian_waypoints_planner_B.value_h + 1];

    cartesian_waypoints_planner_B.R[cartesian_waypoints_planner_B.subsa_idx_1 +

      2] = cartesian_waypoints_planner_B.R_a[3 *

      cartesian_waypoints_planner_B.value_h + 2];

    cartesian_waypoints_planner_B.R[cartesian_waypoints_planner_B.value_h + 12] =

      cartesian_waypoints_planner_B.tempR[cartesian_waypoints_planner_B.value_h

      + 6] * cartesian_waypoints_planner_B.T2[14] +

      (cartesian_waypoints_planner_B.tempR[cartesian_waypoints_planner_B.value_h

       + 3] * cartesian_waypoints_planner_B.T2[13] +

       cartesian_waypoints_planner_B.tempR[cartesian_waypoints_planner_B.value_h]

       * cartesian_waypoints_planner_B.T2[12]);

  }



  cartesian_waypoints_planner_B.R[3] = 0.0;

  cartesian_waypoints_planner_B.R[7] = 0.0;

  cartesian_waypoints_planner_B.R[11] = 0.0;

  cartesian_waypoints_planner_B.R[15] = 1.0;

  for (cartesian_waypoints_planner_B.value_h = 0;

       cartesian_waypoints_planner_B.value_h < 4;

       cartesian_waypoints_planner_B.value_h++) {

    for (cartesian_waypoints_planner_B.subsa_idx_1 = 0;

         cartesian_waypoints_planner_B.subsa_idx_1 < 4;

         cartesian_waypoints_planner_B.subsa_idx_1++) {

      cartesian_waypoints_planner_B.i = cartesian_waypoints_planner_B.value_h <<

        2;

      cartesian_waypoints_planner_B.j =

        cartesian_waypoints_planner_B.subsa_idx_1 +

        cartesian_waypoints_planner_B.i;

      cartesian_waypoints_planner_B.T2[cartesian_waypoints_planner_B.j] = 0.0;

      cartesian_waypoints_planner_B.T2[cartesian_waypoints_planner_B.j] +=

        cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.i] *

        cartesian_waypoints_planner_B.R[cartesian_waypoints_planner_B.subsa_idx_1];

      cartesian_waypoints_planner_B.T2[cartesian_waypoints_planner_B.j] +=

        cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.i + 1] *

        cartesian_waypoints_planner_B.R[cartesian_waypoints_planner_B.subsa_idx_1

        + 4];

      cartesian_waypoints_planner_B.T2[cartesian_waypoints_planner_B.j] +=

        cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.i + 2] *

        cartesian_waypoints_planner_B.R[cartesian_waypoints_planner_B.subsa_idx_1

        + 8];

      cartesian_waypoints_planner_B.T2[cartesian_waypoints_planner_B.j] +=

        cartesian_waypoints_planner_B.out[cartesian_waypoints_planner_B.i + 3] *

        cartesian_waypoints_planner_B.R[cartesian_waypoints_planner_B.subsa_idx_1

        + 12];

    }

  }



  // MATLABSystem: '<S13>/Set Parameter' incorporates:

  //   Delay: '<Root>/Delay2'

  //   MATLAB Function: '<Root>/MATLAB Function2'



  ParamSet_cartesian_waypoints_planner_364.set_parameter

    (cartesian_waypoints_planner_DW.Delay2_DSTATE[2]);



  // MATLABSystem: '<S13>/Set Parameter1' incorporates:

  //   Delay: '<Root>/Delay2'

  //   MATLAB Function: '<Root>/MATLAB Function2'



  ParamSet_cartesian_waypoints_planner_365.set_parameter

    (cartesian_waypoints_planner_DW.Delay2_DSTATE[3]);



  // MATLABSystem: '<S13>/Set Parameter2' incorporates:

  //   Delay: '<Root>/Delay2'

  //   MATLAB Function: '<Root>/MATLAB Function2'



  ParamSet_cartesian_waypoints_planner_366.set_parameter

    (cartesian_waypoints_planner_DW.Delay2_DSTATE[4]);



  // MATLABSystem: '<S13>/Set Parameter3' incorporates:

  //   Delay: '<Root>/Delay2'

  //   MATLAB Function: '<Root>/MATLAB Function2'



  ParamSet_cartesian_waypoints_planner_371.set_parameter

    (cartesian_waypoints_planner_DW.Delay2_DSTATE[5]);



  // MATLABSystem: '<S13>/Set Parameter4' incorporates:

  //   Delay: '<Root>/Delay2'

  //   MATLAB Function: '<Root>/MATLAB Function2'



  ParamSet_cartesian_waypoints_planner_372.set_parameter

    (cartesian_waypoints_planner_DW.Delay2_DSTATE[1]);



  // MATLABSystem: '<S13>/Set Parameter5' incorporates:

  //   Delay: '<Root>/Delay2'

  //   MATLAB Function: '<Root>/MATLAB Function2'



  ParamSet_cartesian_waypoints_planner_373.set_parameter

    (cartesian_waypoints_planner_DW.Delay2_DSTATE[0]);



  // MATLAB Function: '<S9>/MATLAB Function5'

  cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c =

    cartesian_waypoints_planner_B.T2[4] + cartesian_waypoints_planner_B.T2[1];

  cartesian_waypoints_planner_B.b_n_g = cartesian_waypoints_planner_B.T2[8] +

    cartesian_waypoints_planner_B.T2[2];

  cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx =

    cartesian_waypoints_planner_B.T2[6] - cartesian_waypoints_planner_B.T2[9];

  cartesian_waypoints_planner_B.d_n = cartesian_waypoints_planner_B.T2[9] +

    cartesian_waypoints_planner_B.T2[6];

  cartesian_waypoints_planner_B.K24 = cartesian_waypoints_planner_B.T2[8] -

    cartesian_waypoints_planner_B.T2[2];

  cartesian_waypoints_planner_B.dp = cartesian_waypoints_planner_B.T2[1] -

    cartesian_waypoints_planner_B.T2[4];

  cartesian_waypoints_planner_B.out[0] = ((cartesian_waypoints_planner_B.T2[0] -

    cartesian_waypoints_planner_B.T2[5]) - cartesian_waypoints_planner_B.T2[10])

    / 3.0;

  cartesian_waypoints_planner_B.out[4] =

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c / 3.0;

  cartesian_waypoints_planner_B.out[8] = cartesian_waypoints_planner_B.b_n_g /

    3.0;

  cartesian_waypoints_planner_B.out[12] =

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx / 3.0;

  cartesian_waypoints_planner_B.out[1] =

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c / 3.0;

  cartesian_waypoints_planner_B.out[5] = ((cartesian_waypoints_planner_B.T2[5] -

    cartesian_waypoints_planner_B.T2[0]) - cartesian_waypoints_planner_B.T2[10])

    / 3.0;

  cartesian_waypoints_planner_B.out[9] = cartesian_waypoints_planner_B.d_n / 3.0;

  cartesian_waypoints_planner_B.out[13] = cartesian_waypoints_planner_B.K24 /

    3.0;

  cartesian_waypoints_planner_B.out[2] = cartesian_waypoints_planner_B.b_n_g /

    3.0;

  cartesian_waypoints_planner_B.out[6] = cartesian_waypoints_planner_B.d_n / 3.0;

  cartesian_waypoints_planner_B.out[10] = ((cartesian_waypoints_planner_B.T2[10]

    - cartesian_waypoints_planner_B.T2[0]) - cartesian_waypoints_planner_B.T2[5])

    / 3.0;

  cartesian_waypoints_planner_B.out[14] = cartesian_waypoints_planner_B.dp / 3.0;

  cartesian_waypoints_planner_B.out[3] =

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu_cx / 3.0;

  cartesian_waypoints_planner_B.out[7] = cartesian_waypoints_planner_B.K24 / 3.0;

  cartesian_waypoints_planner_B.out[11] = cartesian_waypoints_planner_B.dp / 3.0;

  cartesian_waypoints_planner_B.out[15] = ((cartesian_waypoints_planner_B.T2[0]

    + cartesian_waypoints_planner_B.T2[5]) + cartesian_waypoints_planner_B.T2[10])

    / 3.0;

  cartesian_waypoints_planner_eig(cartesian_waypoints_planner_B.out,

    cartesian_waypoints_planner_B.eigVec, cartesian_waypoints_planner_B.eigVal);

  cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[0] =

    cartesian_waypoints_planner_B.eigVal[0].re;

  cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[1] =

    cartesian_waypoints_planner_B.eigVal[1].re;

  cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[2] =

    cartesian_waypoints_planner_B.eigVal[2].re;

  cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[3] =

    cartesian_waypoints_planner_B.eigVal[3].re;

  if (!rtIsNaN(cartesian_waypoints_planner_B.eigVal[0].re)) {

    cartesian_waypoints_planner_B.value_h = 1;

  } else {

    cartesian_waypoints_planner_B.value_h = 0;

    cartesian_waypoints_planner_B.subsa_idx_1 = 2;

    exitg1 = false;

    while ((!exitg1) && (cartesian_waypoints_planner_B.subsa_idx_1 < 5)) {

      if (!rtIsNaN

          (cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[cartesian_waypoints_planner_B.subsa_idx_1

           - 1])) {

        cartesian_waypoints_planner_B.value_h =

          cartesian_waypoints_planner_B.subsa_idx_1;

        exitg1 = true;

      } else {

        cartesian_waypoints_planner_B.subsa_idx_1++;

      }

    }

  }



  if (cartesian_waypoints_planner_B.value_h != 0) {

    cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c =

      cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[cartesian_waypoints_planner_B.value_h

      - 1];

    cartesian_waypoints_planner_B.subsa_idx_1 =

      cartesian_waypoints_planner_B.value_h - 1;

    while (cartesian_waypoints_planner_B.value_h + 1 < 5) {

      if (cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c <

          cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[cartesian_waypoints_planner_B.value_h])

      {

        cartesian_waypoints_planner_B.rtb_TmpSignalConversionAtSFu__c =

          cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[cartesian_waypoints_planner_B.value_h];

        cartesian_waypoints_planner_B.subsa_idx_1 =

          cartesian_waypoints_planner_B.value_h;

      }



      cartesian_waypoints_planner_B.value_h++;

    }



    cartesian_waypoints_planner_B.value_h =

      cartesian_waypoints_planner_B.subsa_idx_1;

  }



  cartesian_waypoints_planner_B.value_h <<= 2;

  cartesian_waypoints_planner_B.subsa_idx_1 =

    cartesian_waypoints_planner_B.value_h + 3;

  cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[0] =

    cartesian_waypoints_planner_B.eigVec[cartesian_waypoints_planner_B.subsa_idx_1]

    .re;

  cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[1] =

    cartesian_waypoints_planner_B.eigVec[cartesian_waypoints_planner_B.value_h].

    re;

  cartesian_waypoints_planner_B.i = cartesian_waypoints_planner_B.value_h + 1;

  cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[2] =

    cartesian_waypoints_planner_B.eigVec[cartesian_waypoints_planner_B.i].re;

  cartesian_waypoints_planner_B.j = cartesian_waypoints_planner_B.value_h + 2;

  cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[3] =

    cartesian_waypoints_planner_B.eigVec[cartesian_waypoints_planner_B.j].re;

  if (cartesian_waypoints_planner_B.eigVec[cartesian_waypoints_planner_B.subsa_idx_1]

      .re < 0.0) {

    cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[0] =

      -cartesian_waypoints_planner_B.eigVec[cartesian_waypoints_planner_B.subsa_idx_1]

      .re;

    cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[1] =

      -cartesian_waypoints_planner_B.eigVec[cartesian_waypoints_planner_B.value_h]

      .re;

    cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[2] =

      -cartesian_waypoints_planner_B.eigVec[cartesian_waypoints_planner_B.i].re;

    cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[3] =

      -cartesian_waypoints_planner_B.eigVec[cartesian_waypoints_planner_B.j].re;

  }



  // BusAssignment: '<S9>/Bus Assignment1' incorporates:

  //   Constant: '<S14>/Constant'

  //   MATLAB Function: '<S9>/MATLAB Function5'



  cartesian_waypoints_planner_B.BusAssignment1 =

    cartesian_waypoints_planner_P.Constant_Value_b;

  cartesian_waypoints_planner_B.BusAssignment1.Pose.Position.X =

    cartesian_waypoints_planner_B.T2[12];

  cartesian_waypoints_planner_B.BusAssignment1.Pose.Position.Y =

    cartesian_waypoints_planner_B.T2[13];

  cartesian_waypoints_planner_B.BusAssignment1.Pose.Position.Z =

    cartesian_waypoints_planner_B.T2[14];

  cartesian_waypoints_planner_B.BusAssignment1.Pose.Orientation.X =

    cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[0];

  cartesian_waypoints_planner_B.BusAssignment1.Pose.Orientation.Y =

    cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[1];

  cartesian_waypoints_planner_B.BusAssignment1.Pose.Orientation.Z =

    cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[2];

  cartesian_waypoints_planner_B.BusAssignment1.Pose.Orientation.W =

    cartesian_waypoints_planner_B.TmpSignalConversionAtSFunct[3];

  cartesian_waypoints_planner_B.BusAssignment1.Header.Stamp =

    cartesian_waypoints_planner_B.In1.Clock_;



  // Outputs for Atomic SubSystem: '<S9>/Publish1'

  // MATLABSystem: '<S17>/SinkBlock'

  Pub_cartesian_waypoints_planner_425.publish

    (&cartesian_waypoints_planner_B.BusAssignment1);



  // End of Outputs for SubSystem: '<S9>/Publish1'



  // MATLAB Function: '<Root>/MATLAB Function1' incorporates:

  //   BusAssignment: '<Root>/Bus Assignment'

  //   Constant: '<S1>/Constant'



  cartesian_waypoints_planner_B.msg =

    cartesian_waypoints_planner_P.Constant_Value;



  // BusAssignment: '<Root>/Bus Assignment' incorporates:

  //   MATLAB Function: '<Root>/MATLAB Function1'



  cartesian_waypoints_planner_B.msg.TimeFromStart.Sec =

    cartesian_waypoints_planner_B.In1.Clock_.Sec;

  cartesian_waypoints_planner_B.msg.TimeFromStart.Nsec =

    cartesian_waypoints_planner_B.In1.Clock_.Nsec;



  // MATLAB Function: '<Root>/MATLAB Function1'

  cartesian_waypoints_planner_B.msg.Velocities_SL_Info.CurrentLength = 6U;

  cartesian_waypoints_planner_B.msg.Positions_SL_Info.CurrentLength = 6U;

  cartesian_waypoints_planner_B.msg.Accelerations_SL_Info.CurrentLength = 6U;

  for (cartesian_waypoints_planner_B.value_h = 0;

       cartesian_waypoints_planner_B.value_h < 6;

       cartesian_waypoints_planner_B.value_h++) {

    cartesian_waypoints_planner_B.msg.Velocities[cartesian_waypoints_planner_B.value_h]

      = cartesian_waypoints_planner_B.qd[cartesian_waypoints_planner_B.value_h];

    cartesian_waypoints_planner_B.msg.Positions[cartesian_waypoints_planner_B.value_h]

      = cartesian_waypoints_planner_B.q[cartesian_waypoints_planner_B.value_h];

    cartesian_waypoints_planner_B.msg.Accelerations[cartesian_waypoints_planner_B.value_h]

      = cartesian_waypoints_planner_B.qdd[cartesian_waypoints_planner_B.value_h];

  }



  // Outputs for Atomic SubSystem: '<Root>/Publish'

  // MATLABSystem: '<S8>/SinkBlock'

  Pub_cartesian_waypoints_planner_392.publish(&cartesian_waypoints_planner_B.msg);



  // End of Outputs for SubSystem: '<Root>/Publish'



  // Update for Delay: '<Root>/Delay' incorporates:

  //   MATLABSystem: '<S2>/MATLAB System'



  for (cartesian_waypoints_planner_B.i = 0; cartesian_waypoints_planner_B.i < 6;

       cartesian_waypoints_planner_B.i++) {

    cartesian_waypoints_planner_DW.Delay_DSTATE[cartesian_waypoints_planner_B.i]

      =

      cartesian_waypoints_planner_B.b_varargout_1[cartesian_waypoints_planner_B.i];

  }



  // End of Update for Delay: '<Root>/Delay'

}



// Model initialize function

void cartesian_waypoints_planner_initialize(void)

{

  // Registration code



  // initialize non-finites

  rt_InitInfAndNaN(sizeof(real_T));



  {

    robotics_slmanip_internal_blo_T *obj;

    b_inverseKinematics_cartesian_T *obj_0;

    h_robotics_core_internal_Damp_T *obj_1;

    static const char_T tmp[6] = { '/', 'c', 'l', 'o', 'c', 'k' };



    static const char_T tmp_0[21] = { '/', 'c', 'a', 'r', 't', 'e', 's', 'i',

      'a', 'n', '_', 't', 'r', 'a', 'j', 'e', 'c', 't', 'o', 'r', 'y' };



    static const char_T tmp_1[17] = { '/', 'j', 'o', 'i', 'n', 't', '_', 't',

      'r', 'a', 'j', 'e', 'c', 't', 'o', 'r', 'y' };



    static const char_T tmp_2[9] = { '/', 'n', '_', 'p', 'o', 'i', 'n', 't', 's'

    };



    static const char_T tmp_3[5] = { '/', 'p', 'i', '_', 'x' };



    static const char_T tmp_4[5] = { '/', 'p', 'i', '_', 'y' };



    static const char_T tmp_5[5] = { '/', 'p', 'i', '_', 'z' };



    static const char_T tmp_6[5] = { '/', 'o', 'i', '_', 'x' };



    static const char_T tmp_7[5] = { '/', 'o', 'i', '_', 'y' };



    static const char_T tmp_8[5] = { '/', 'o', 'i', '_', 'z' };



    static const char_T tmp_9[5] = { '/', 'o', 'i', '_', 'w' };



    static const char_T tmp_a[5] = { '/', 'p', 'f', '_', 'x' };



    static const char_T tmp_b[5] = { '/', 'p', 'f', '_', 'y' };



    static const char_T tmp_c[5] = { '/', 'p', 'f', '_', 'z' };



    static const char_T tmp_d[5] = { '/', 'o', 'f', '_', 'x' };



    static const char_T tmp_e[5] = { '/', 'o', 'f', '_', 'y' };



    static const char_T tmp_f[5] = { '/', 'o', 'f', '_', 'z' };



    static const char_T tmp_g[5] = { '/', 'o', 'f', '_', 'w' };



    static const char_T tmp_h[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',

      'o', 'x' };



    static const char_T tmp_i[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',

      'o', 'y' };



    static const char_T tmp_j[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',

      'o', 'z' };



    static const char_T tmp_k[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',

      'p', 'x' };



    static const char_T tmp_l[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',

      'p', 'y' };



    static const char_T tmp_m[10] = { '/', 'w', 'e', 'i', 'g', 'h', 't', '_',

      'p', 'z' };



    static const char_T tmp_n[12] = { '/', 's', 't', 'a', 'r', 't', '_', 'd',

      'e', 'l', 'a', 'y' };



    static const char_T tmp_o[12] = { '/', 'm', 'a', 'x', '_', 'a', 'n', 'g',

      '_', 'v', 'e', 'l' };



    static const char_T tmp_p[12] = { '/', 'm', 'a', 'x', '_', 'a', 'n', 'g',

      '_', 'a', 'c', 'c' };



    static const char_T tmp_q[11] = { '/', 'q', '3', '_', 'i', 'n', 'i', 't',

      'i', 'a', 'l' };



    static const char_T tmp_r[11] = { '/', 'q', '4', '_', 'i', 'n', 'i', 't',

      'i', 'a', 'l' };



    static const char_T tmp_s[11] = { '/', 'q', '5', '_', 'i', 'n', 'i', 't',

      'i', 'a', 'l' };



    static const char_T tmp_t[11] = { '/', 'q', '6', '_', 'i', 'n', 'i', 't',

      'i', 'a', 'l' };



    static const char_T tmp_u[11] = { '/', 'q', '2', '_', 'i', 'n', 'i', 't',

      'i', 'a', 'l' };



    static const char_T tmp_v[11] = { '/', 'q', '1', '_', 'i', 'n', 'i', 't',

      'i', 'a', 'l' };



    // InitializeConditions for Delay: '<Root>/Delay2'

    memcpy(&cartesian_waypoints_planner_DW.Delay2_DSTATE[0],

           &cartesian_waypoints_planner_P.Delay2_InitialCondition[0], 120U *

           sizeof(real_T));



    // InitializeConditions for Delay: '<Root>/Delay3'

    cartesian_waypoints_planner_DW.Delay3_DSTATE =

      cartesian_waypoints_planner_P.Delay3_InitialCondition;



    // InitializeConditions for Delay: '<Root>/Delay1'

    cartesian_waypoints_planner_DW.Delay1_DSTATE =

      cartesian_waypoints_planner_P.Delay1_InitialCondition;



    // SystemInitialize for Atomic SubSystem: '<Root>/Subscribe1'

    // SystemInitialize for Enabled SubSystem: '<S10>/Enabled Subsystem'

    // SystemInitialize for Outport: '<S18>/Out1'

    cartesian_waypoints_planner_B.In1 = cartesian_waypoints_planner_P.Out1_Y0;



    // End of SystemInitialize for SubSystem: '<S10>/Enabled Subsystem'



    // Start for MATLABSystem: '<S10>/SourceBlock'

    cartesian_waypoints_planner_DW.obj_pt.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_pt.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 6;

         cartesian_waypoints_planner_B.i_j++) {

      // InitializeConditions for Delay: '<Root>/Delay'

      cartesian_waypoints_planner_DW.Delay_DSTATE[cartesian_waypoints_planner_B.i_j]

        =

        cartesian_waypoints_planner_P.Delay_InitialCondition[cartesian_waypoints_planner_B.i_j];



      // Start for MATLABSystem: '<S10>/SourceBlock'

      cartesian_waypoints_planner_B.cv6[cartesian_waypoints_planner_B.i_j] =

        tmp[cartesian_waypoints_planner_B.i_j];

    }



    // Start for MATLABSystem: '<S10>/SourceBlock'

    cartesian_waypoints_planner_B.cv6[6] = '\x00';

    Sub_cartesian_waypoints_planner_384.createSubscriber

      (cartesian_waypoints_planner_B.cv6, 1);

    cartesian_waypoints_planner_DW.obj_pt.isSetupComplete = true;



    // End of SystemInitialize for SubSystem: '<Root>/Subscribe1'



    // SystemInitialize for Atomic SubSystem: '<S9>/Publish1'

    // Start for MATLABSystem: '<S17>/SinkBlock'

    cartesian_waypoints_planner_DW.obj_h5.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_h5.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 21;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv[cartesian_waypoints_planner_B.i_j] =

        tmp_0[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv[21] = '\x00';

    Pub_cartesian_waypoints_planner_425.createPublisher

      (cartesian_waypoints_planner_B.cv, 1);

    cartesian_waypoints_planner_DW.obj_h5.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S17>/SinkBlock'

    // End of SystemInitialize for SubSystem: '<S9>/Publish1'



    // SystemInitialize for Atomic SubSystem: '<Root>/Publish'

    // Start for MATLABSystem: '<S8>/SinkBlock'

    cartesian_waypoints_planner_DW.obj_b.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_b.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 17;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv1[cartesian_waypoints_planner_B.i_j] =

        tmp_1[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv1[17] = '\x00';

    Pub_cartesian_waypoints_planner_392.createPublisher

      (cartesian_waypoints_planner_B.cv1, 1);

    cartesian_waypoints_planner_DW.obj_b.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S8>/SinkBlock'

    // End of SystemInitialize for SubSystem: '<Root>/Publish'



    // Start for MATLABSystem: '<S11>/Get Parameter'

    cartesian_waypoints_planner_DW.obj_eq.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_eq.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 9;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv5[cartesian_waypoints_planner_B.i_j] =

        tmp_2[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv5[9] = '\x00';

    ParamGet_cartesian_waypoints_planner_346.initialize

      (cartesian_waypoints_planner_B.cv5);

    ParamGet_cartesian_waypoints_planner_346.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_346.set_initial_value(3);

    cartesian_waypoints_planner_DW.obj_eq.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S11>/Get Parameter'



    // Start for MATLABSystem: '<S19>/Get Parameter'

    cartesian_waypoints_planner_DW.obj_i.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_i.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 5;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =

        tmp_3[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv7[5] = '\x00';

    ParamGet_cartesian_waypoints_planner_288.initialize

      (cartesian_waypoints_planner_B.cv7);

    ParamGet_cartesian_waypoints_planner_288.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_288.set_initial_value(0.0);

    cartesian_waypoints_planner_DW.obj_i.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S19>/Get Parameter'



    // Start for MATLABSystem: '<S19>/Get Parameter1'

    cartesian_waypoints_planner_DW.obj_db.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_db.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 5;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =

        tmp_4[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv7[5] = '\x00';

    ParamGet_cartesian_waypoints_planner_289.initialize

      (cartesian_waypoints_planner_B.cv7);

    ParamGet_cartesian_waypoints_planner_289.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_289.set_initial_value(0.0);

    cartesian_waypoints_planner_DW.obj_db.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S19>/Get Parameter1'



    // Start for MATLABSystem: '<S19>/Get Parameter2'

    cartesian_waypoints_planner_DW.obj_oq.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_oq.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 5;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =

        tmp_5[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv7[5] = '\x00';

    ParamGet_cartesian_waypoints_planner_290.initialize

      (cartesian_waypoints_planner_B.cv7);

    ParamGet_cartesian_waypoints_planner_290.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_290.set_initial_value(0.99);

    cartesian_waypoints_planner_DW.obj_oq.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S19>/Get Parameter2'



    // Start for MATLABSystem: '<S19>/Get Parameter3'

    cartesian_waypoints_planner_DW.obj_es.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_es.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 5;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =

        tmp_6[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv7[5] = '\x00';

    ParamGet_cartesian_waypoints_planner_291.initialize

      (cartesian_waypoints_planner_B.cv7);

    ParamGet_cartesian_waypoints_planner_291.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_291.set_initial_value(0.0);

    cartesian_waypoints_planner_DW.obj_es.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S19>/Get Parameter3'



    // Start for MATLABSystem: '<S19>/Get Parameter4'

    cartesian_waypoints_planner_DW.obj_f4.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_f4.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 5;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =

        tmp_7[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv7[5] = '\x00';

    ParamGet_cartesian_waypoints_planner_292.initialize

      (cartesian_waypoints_planner_B.cv7);

    ParamGet_cartesian_waypoints_planner_292.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_292.set_initial_value(0.0);

    cartesian_waypoints_planner_DW.obj_f4.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S19>/Get Parameter4'



    // Start for MATLABSystem: '<S19>/Get Parameter5'

    cartesian_waypoints_planner_DW.obj_en.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_en.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 5;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =

        tmp_8[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv7[5] = '\x00';

    ParamGet_cartesian_waypoints_planner_293.initialize

      (cartesian_waypoints_planner_B.cv7);

    ParamGet_cartesian_waypoints_planner_293.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_293.set_initial_value(0.0);

    cartesian_waypoints_planner_DW.obj_en.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S19>/Get Parameter5'



    // Start for MATLABSystem: '<S19>/Get Parameter6'

    cartesian_waypoints_planner_DW.obj_l0.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_l0.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 5;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =

        tmp_9[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv7[5] = '\x00';

    ParamGet_cartesian_waypoints_planner_294.initialize

      (cartesian_waypoints_planner_B.cv7);

    ParamGet_cartesian_waypoints_planner_294.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_294.set_initial_value(1.0);

    cartesian_waypoints_planner_DW.obj_l0.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S19>/Get Parameter6'



    // Start for MATLABSystem: '<S20>/Get Parameter'

    cartesian_waypoints_planner_DW.obj_mb.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_mb.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 5;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =

        tmp_a[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv7[5] = '\x00';

    ParamGet_cartesian_waypoints_planner_300.initialize

      (cartesian_waypoints_planner_B.cv7);

    ParamGet_cartesian_waypoints_planner_300.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_300.set_initial_value(0.3);

    cartesian_waypoints_planner_DW.obj_mb.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S20>/Get Parameter'



    // Start for MATLABSystem: '<S20>/Get Parameter1'

    cartesian_waypoints_planner_DW.obj_c.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_c.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 5;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =

        tmp_b[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv7[5] = '\x00';

    ParamGet_cartesian_waypoints_planner_301.initialize

      (cartesian_waypoints_planner_B.cv7);

    ParamGet_cartesian_waypoints_planner_301.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_301.set_initial_value(0.3);

    cartesian_waypoints_planner_DW.obj_c.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S20>/Get Parameter1'



    // Start for MATLABSystem: '<S20>/Get Parameter2'

    cartesian_waypoints_planner_DW.obj_f.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_f.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 5;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =

        tmp_c[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv7[5] = '\x00';

    ParamGet_cartesian_waypoints_planner_302.initialize

      (cartesian_waypoints_planner_B.cv7);

    ParamGet_cartesian_waypoints_planner_302.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_302.set_initial_value(0.3);

    cartesian_waypoints_planner_DW.obj_f.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S20>/Get Parameter2'



    // Start for MATLABSystem: '<S20>/Get Parameter3'

    cartesian_waypoints_planner_DW.obj_e0.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_e0.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 5;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =

        tmp_d[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv7[5] = '\x00';

    ParamGet_cartesian_waypoints_planner_303.initialize

      (cartesian_waypoints_planner_B.cv7);

    ParamGet_cartesian_waypoints_planner_303.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_303.set_initial_value(0.39);

    cartesian_waypoints_planner_DW.obj_e0.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S20>/Get Parameter3'



    // Start for MATLABSystem: '<S20>/Get Parameter4'

    cartesian_waypoints_planner_DW.obj_a.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_a.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 5;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =

        tmp_e[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv7[5] = '\x00';

    ParamGet_cartesian_waypoints_planner_304.initialize

      (cartesian_waypoints_planner_B.cv7);

    ParamGet_cartesian_waypoints_planner_304.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_304.set_initial_value(0.89);

    cartesian_waypoints_planner_DW.obj_a.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S20>/Get Parameter4'



    // Start for MATLABSystem: '<S20>/Get Parameter5'

    cartesian_waypoints_planner_DW.obj_p.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_p.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 5;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =

        tmp_f[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv7[5] = '\x00';

    ParamGet_cartesian_waypoints_planner_305.initialize

      (cartesian_waypoints_planner_B.cv7);

    ParamGet_cartesian_waypoints_planner_305.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_305.set_initial_value(0.2);

    cartesian_waypoints_planner_DW.obj_p.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S20>/Get Parameter5'



    // Start for MATLABSystem: '<S20>/Get Parameter6'

    cartesian_waypoints_planner_DW.obj_l.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_l.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 5;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv7[cartesian_waypoints_planner_B.i_j] =

        tmp_g[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv7[5] = '\x00';

    ParamGet_cartesian_waypoints_planner_306.initialize

      (cartesian_waypoints_planner_B.cv7);

    ParamGet_cartesian_waypoints_planner_306.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_306.set_initial_value(-0.12);

    cartesian_waypoints_planner_DW.obj_l.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S20>/Get Parameter6'



    // Start for MATLABSystem: '<S12>/Get Parameter'

    cartesian_waypoints_planner_DW.obj_e.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_e.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 10;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv4[cartesian_waypoints_planner_B.i_j] =

        tmp_h[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv4[10] = '\x00';

    ParamGet_cartesian_waypoints_planner_312.initialize

      (cartesian_waypoints_planner_B.cv4);

    ParamGet_cartesian_waypoints_planner_312.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_312.set_initial_value(0.0);

    cartesian_waypoints_planner_DW.obj_e.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S12>/Get Parameter'



    // Start for MATLABSystem: '<S12>/Get Parameter1'

    cartesian_waypoints_planner_DW.obj_d.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_d.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 10;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv4[cartesian_waypoints_planner_B.i_j] =

        tmp_i[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv4[10] = '\x00';

    ParamGet_cartesian_waypoints_planner_313.initialize

      (cartesian_waypoints_planner_B.cv4);

    ParamGet_cartesian_waypoints_planner_313.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_313.set_initial_value(0.0);

    cartesian_waypoints_planner_DW.obj_d.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S12>/Get Parameter1'



    // Start for MATLABSystem: '<S12>/Get Parameter2'

    cartesian_waypoints_planner_DW.obj_mt.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_mt.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 10;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv4[cartesian_waypoints_planner_B.i_j] =

        tmp_j[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv4[10] = '\x00';

    ParamGet_cartesian_waypoints_planner_314.initialize

      (cartesian_waypoints_planner_B.cv4);

    ParamGet_cartesian_waypoints_planner_314.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_314.set_initial_value(0.0);

    cartesian_waypoints_planner_DW.obj_mt.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S12>/Get Parameter2'



    // Start for MATLABSystem: '<S12>/Get Parameter3'

    cartesian_waypoints_planner_DW.obj_h.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_h.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 10;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv4[cartesian_waypoints_planner_B.i_j] =

        tmp_k[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv4[10] = '\x00';

    ParamGet_cartesian_waypoints_planner_315.initialize

      (cartesian_waypoints_planner_B.cv4);

    ParamGet_cartesian_waypoints_planner_315.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_315.set_initial_value(1.0);

    cartesian_waypoints_planner_DW.obj_h.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S12>/Get Parameter3'



    // Start for MATLABSystem: '<S12>/Get Parameter4'

    cartesian_waypoints_planner_DW.obj_mf.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_mf.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 10;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv4[cartesian_waypoints_planner_B.i_j] =

        tmp_l[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv4[10] = '\x00';

    ParamGet_cartesian_waypoints_planner_316.initialize

      (cartesian_waypoints_planner_B.cv4);

    ParamGet_cartesian_waypoints_planner_316.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_316.set_initial_value(1.0);

    cartesian_waypoints_planner_DW.obj_mf.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S12>/Get Parameter4'



    // Start for MATLABSystem: '<S12>/Get Parameter5'

    cartesian_waypoints_planner_DW.obj_o.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_o.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 10;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv4[cartesian_waypoints_planner_B.i_j] =

        tmp_m[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv4[10] = '\x00';

    ParamGet_cartesian_waypoints_planner_317.initialize

      (cartesian_waypoints_planner_B.cv4);

    ParamGet_cartesian_waypoints_planner_317.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_317.set_initial_value(1.0);

    cartesian_waypoints_planner_DW.obj_o.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S12>/Get Parameter5'

    emxInitStruct_robotics_slmanip_(&cartesian_waypoints_planner_DW.obj);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_1);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_50);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_49);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_48);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_47);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_46);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_45);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_44);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_43);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_42);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_41);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_40);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_39);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_38);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_37);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_36);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_35);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_34);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_33);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_32);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_31);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_30);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_29);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_28);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_27);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_26);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_25);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_24);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_23);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_22);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_21);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_20);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_19);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_18);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_17);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_16);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_15);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_14);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_13);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_12);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_11);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_10);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_9);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_8);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_7);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_6);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_5);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_4);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_3);

    emxInitStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_2);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_51);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_82);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_81);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_80);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_79);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_78);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_77);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_76);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_75);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_74);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_73);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_72);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_71);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_70);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_69);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_68);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_67);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_66);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_65);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_64);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_63);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_62);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_61);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_60);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_59);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_58);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_57);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_56);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_55);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_54);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_53);

    emxInitStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_52);

    emxInitStruct_x_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_83);

    emxInitStruct_x_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_84);

    emxInitStruct_f_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_85);

    emxInitStruct_h_robotics_core_i(&cartesian_waypoints_planner_DW.gobj_86);

    emxInitStruct_h_robotics_core_i(&cartesian_waypoints_planner_DW.gobj_87);

    emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_88);

    emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_103);

    emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_102);

    emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_101);

    emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_100);

    emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_99);

    emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_98);

    emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_97);

    emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_96);

    emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_95);

    emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_94);

    emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_93);

    emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_92);

    emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_91);

    emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_90);

    emxInitStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_89);



    // Start for MATLABSystem: '<S2>/MATLAB System'

    cartesian_waypoints_planner_DW.obj.IKInternal.matlabCodegenIsDeleted = true;

    cartesian_waypoints_planner_DW.obj.matlabCodegenIsDeleted = true;

    cartesian_wa_eml_rand_mt19937ar(cartesian_waypoints_planner_DW.state_m);

    cartesian_waypoints_planner_DW.obj.isInitialized = 0;

    cartesian_waypoints_planner_DW.obj.matlabCodegenIsDeleted = false;

    obj = &cartesian_waypoints_planner_DW.obj;

    cartesian_waypoints_planner_DW.obj.isInitialized = 1;

    car_RigidBodyTree_RigidBodyTree

      (&cartesian_waypoints_planner_DW.obj.TreeInternal,

       &cartesian_waypoints_planner_DW.gobj_90,

       &cartesian_waypoints_planner_DW.gobj_91,

       &cartesian_waypoints_planner_DW.gobj_92,

       &cartesian_waypoints_planner_DW.gobj_93,

       &cartesian_waypoints_planner_DW.gobj_94,

       &cartesian_waypoints_planner_DW.gobj_95,

       &cartesian_waypoints_planner_DW.gobj_96,

       &cartesian_waypoints_planner_DW.gobj_89);

    obj_0 = &cartesian_waypoints_planner_DW.obj.IKInternal;

    obj_0->isInitialized = 0;

    inverseKinematics_set_RigidBody(obj_0, &obj->TreeInternal,

      &cartesian_waypoints_planner_DW.gobj_69,

      &cartesian_waypoints_planner_DW.gobj_70,

      &cartesian_waypoints_planner_DW.gobj_71,

      &cartesian_waypoints_planner_DW.gobj_72,

      &cartesian_waypoints_planner_DW.gobj_73,

      &cartesian_waypoints_planner_DW.gobj_74,

      &cartesian_waypoints_planner_DW.gobj_75,

      &cartesian_waypoints_planner_DW.gobj_76,

      &cartesian_waypoints_planner_DW.gobj_77,

      &cartesian_waypoints_planner_DW.gobj_78,

      &cartesian_waypoints_planner_DW.gobj_79,

      &cartesian_waypoints_planner_DW.gobj_80,

      &cartesian_waypoints_planner_DW.gobj_81,

      &cartesian_waypoints_planner_DW.gobj_82,

      &cartesian_waypoints_planner_DW.gobj_51,

      &cartesian_waypoints_planner_DW.gobj_28,

      &cartesian_waypoints_planner_DW.gobj_29,

      &cartesian_waypoints_planner_DW.gobj_30,

      &cartesian_waypoints_planner_DW.gobj_31,

      &cartesian_waypoints_planner_DW.gobj_32,

      &cartesian_waypoints_planner_DW.gobj_33,

      &cartesian_waypoints_planner_DW.gobj_34,

      &cartesian_waypoints_planner_DW.gobj_35,

      &cartesian_waypoints_planner_DW.gobj_36,

      &cartesian_waypoints_planner_DW.gobj_37,

      &cartesian_waypoints_planner_DW.gobj_38,

      &cartesian_waypoints_planner_DW.gobj_39,

      &cartesian_waypoints_planner_DW.gobj_40,

      &cartesian_waypoints_planner_DW.gobj_41,

      &cartesian_waypoints_planner_DW.gobj_42,

      &cartesian_waypoints_planner_DW.gobj_43,

      &cartesian_waypoints_planner_DW.gobj_44,

      &cartesian_waypoints_planner_DW.gobj_45,

      &cartesian_waypoints_planner_DW.gobj_46,

      &cartesian_waypoints_planner_DW.gobj_47,

      &cartesian_waypoints_planner_DW.gobj_48,

      &cartesian_waypoints_planner_DW.gobj_49,

      &cartesian_waypoints_planner_DW.gobj_50,

      &cartesian_waypoints_planner_DW.gobj_1,

      &cartesian_waypoints_planner_DW.gobj_27,

      &cartesian_waypoints_planner_DW.gobj_68,

      &cartesian_waypoints_planner_DW.gobj_83);

    obj_0->Solver = DampedBFGSwGradientProjection_D

      (&cartesian_waypoints_planner_DW.gobj_87);

    obj_1 = obj_0->Solver;

    obj_1->MaxNumIteration = 1500.0;

    obj_1->MaxTime = 10.0;

    obj_1->GradientTolerance = 1.0E-7;

    obj_1->SolutionTolerance = 1.0E-6;

    obj_1->ConstraintsOn = true;

    obj_1->RandomRestart = false;

    obj_1->StepTolerance = 1.0E-14;

    obj_0->matlabCodegenIsDeleted = false;



    // Start for MATLABSystem: '<Root>/Get Parameter'

    cartesian_waypoints_planner_DW.obj_it.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_it.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 12;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv2[cartesian_waypoints_planner_B.i_j] =

        tmp_n[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv2[12] = '\x00';

    ParamGet_cartesian_waypoints_planner_378.initialize

      (cartesian_waypoints_planner_B.cv2);

    ParamGet_cartesian_waypoints_planner_378.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_378.set_initial_value(10.0);

    cartesian_waypoints_planner_DW.obj_it.isSetupComplete = true;



    // End of Start for MATLABSystem: '<Root>/Get Parameter'



    // Start for MATLABSystem: '<Root>/Get Parameter1'

    cartesian_waypoints_planner_DW.obj_k.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_k.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 12;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv2[cartesian_waypoints_planner_B.i_j] =

        tmp_o[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv2[12] = '\x00';

    ParamGet_cartesian_waypoints_planner_379.initialize

      (cartesian_waypoints_planner_B.cv2);

    ParamGet_cartesian_waypoints_planner_379.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_379.set_initial_value(1.0);

    cartesian_waypoints_planner_DW.obj_k.isSetupComplete = true;



    // End of Start for MATLABSystem: '<Root>/Get Parameter1'



    // Start for MATLABSystem: '<Root>/Get Parameter2'

    cartesian_waypoints_planner_DW.obj_fn.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_fn.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 12;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv2[cartesian_waypoints_planner_B.i_j] =

        tmp_p[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv2[12] = '\x00';

    ParamGet_cartesian_waypoints_planner_380.initialize

      (cartesian_waypoints_planner_B.cv2);

    ParamGet_cartesian_waypoints_planner_380.initialize_error_codes(0, 1, 2, 3);

    ParamGet_cartesian_waypoints_planner_380.set_initial_value(0.5);

    cartesian_waypoints_planner_DW.obj_fn.isSetupComplete = true;



    // End of Start for MATLABSystem: '<Root>/Get Parameter2'

    emxInitStruct_robotics_slmani_e(&cartesian_waypoints_planner_DW.obj_m);

    emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_1_k);

    emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_16_d);

    emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_15_j);

    emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_14_l);

    emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_13_g);

    emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_12_j);

    emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_11_a);

    emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_10_p);

    emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_9_c);

    emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_8_p);

    emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_7_j);

    emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_6_j);

    emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_5_m);

    emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_4_i);

    emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_3_i);

    emxInitStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_2_g);



    // Start for MATLABSystem: '<S15>/MATLAB System'

    cartesian_waypoints_planner_DW.obj_m.isInitialized = 0;

    cartesian_waypoints_planner_DW.obj_m.isInitialized = 1;

    RigidBodyTree_RigidBodyTree_ev

      (&cartesian_waypoints_planner_DW.obj_m.TreeInternal,

       &cartesian_waypoints_planner_DW.gobj_2_g,

       &cartesian_waypoints_planner_DW.gobj_4_i,

       &cartesian_waypoints_planner_DW.gobj_5_m,

       &cartesian_waypoints_planner_DW.gobj_6_j,

       &cartesian_waypoints_planner_DW.gobj_7_j,

       &cartesian_waypoints_planner_DW.gobj_8_p,

       &cartesian_waypoints_planner_DW.gobj_9_c,

       &cartesian_waypoints_planner_DW.gobj_3_i);



    // Start for MATLABSystem: '<S13>/Set Parameter'

    cartesian_waypoints_planner_DW.obj_fb.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_fb.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 11;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv3[cartesian_waypoints_planner_B.i_j] =

        tmp_q[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv3[11] = '\x00';

    ParamSet_cartesian_waypoints_planner_364.initialize

      (cartesian_waypoints_planner_B.cv3);

    cartesian_waypoints_planner_DW.obj_fb.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S13>/Set Parameter'



    // Start for MATLABSystem: '<S13>/Set Parameter1'

    cartesian_waypoints_planner_DW.obj_dr.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_dr.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 11;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv3[cartesian_waypoints_planner_B.i_j] =

        tmp_r[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv3[11] = '\x00';

    ParamSet_cartesian_waypoints_planner_365.initialize

      (cartesian_waypoints_planner_B.cv3);

    cartesian_waypoints_planner_DW.obj_dr.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S13>/Set Parameter1'



    // Start for MATLABSystem: '<S13>/Set Parameter2'

    cartesian_waypoints_planner_DW.obj_a5.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_a5.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 11;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv3[cartesian_waypoints_planner_B.i_j] =

        tmp_s[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv3[11] = '\x00';

    ParamSet_cartesian_waypoints_planner_366.initialize

      (cartesian_waypoints_planner_B.cv3);

    cartesian_waypoints_planner_DW.obj_a5.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S13>/Set Parameter2'



    // Start for MATLABSystem: '<S13>/Set Parameter3'

    cartesian_waypoints_planner_DW.obj_hu.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_hu.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 11;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv3[cartesian_waypoints_planner_B.i_j] =

        tmp_t[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv3[11] = '\x00';

    ParamSet_cartesian_waypoints_planner_371.initialize

      (cartesian_waypoints_planner_B.cv3);

    cartesian_waypoints_planner_DW.obj_hu.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S13>/Set Parameter3'



    // Start for MATLABSystem: '<S13>/Set Parameter4'

    cartesian_waypoints_planner_DW.obj_o1.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_o1.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 11;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv3[cartesian_waypoints_planner_B.i_j] =

        tmp_u[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv3[11] = '\x00';

    ParamSet_cartesian_waypoints_planner_372.initialize

      (cartesian_waypoints_planner_B.cv3);

    cartesian_waypoints_planner_DW.obj_o1.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S13>/Set Parameter4'



    // Start for MATLABSystem: '<S13>/Set Parameter5'

    cartesian_waypoints_planner_DW.obj_ap.matlabCodegenIsDeleted = false;

    cartesian_waypoints_planner_DW.obj_ap.isInitialized = 1;

    for (cartesian_waypoints_planner_B.i_j = 0;

         cartesian_waypoints_planner_B.i_j < 11;

         cartesian_waypoints_planner_B.i_j++) {

      cartesian_waypoints_planner_B.cv3[cartesian_waypoints_planner_B.i_j] =

        tmp_v[cartesian_waypoints_planner_B.i_j];

    }



    cartesian_waypoints_planner_B.cv3[11] = '\x00';

    ParamSet_cartesian_waypoints_planner_373.initialize

      (cartesian_waypoints_planner_B.cv3);

    cartesian_waypoints_planner_DW.obj_ap.isSetupComplete = true;



    // End of Start for MATLABSystem: '<S13>/Set Parameter5'

  }

}



// Model terminate function

void cartesian_waypoints_planner_terminate(void)

{

  // Terminate for MATLABSystem: '<S11>/Get Parameter'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_eq);



  // Terminate for MATLABSystem: '<S19>/Get Parameter'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_i);



  // Terminate for MATLABSystem: '<S19>/Get Parameter1'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_db);



  // Terminate for MATLABSystem: '<S19>/Get Parameter2'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_oq);



  // Terminate for MATLABSystem: '<S19>/Get Parameter3'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_es);



  // Terminate for MATLABSystem: '<S19>/Get Parameter4'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_f4);



  // Terminate for MATLABSystem: '<S19>/Get Parameter5'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_en);



  // Terminate for MATLABSystem: '<S19>/Get Parameter6'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_l0);



  // Terminate for MATLABSystem: '<S20>/Get Parameter'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_mb);



  // Terminate for MATLABSystem: '<S20>/Get Parameter1'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_c);



  // Terminate for MATLABSystem: '<S20>/Get Parameter2'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_f);



  // Terminate for MATLABSystem: '<S20>/Get Parameter3'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_e0);



  // Terminate for MATLABSystem: '<S20>/Get Parameter4'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_a);



  // Terminate for MATLABSystem: '<S20>/Get Parameter5'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_p);



  // Terminate for MATLABSystem: '<S20>/Get Parameter6'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_l);



  // Terminate for MATLABSystem: '<S12>/Get Parameter'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_e);



  // Terminate for MATLABSystem: '<S12>/Get Parameter1'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_d);



  // Terminate for MATLABSystem: '<S12>/Get Parameter2'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_mt);



  // Terminate for MATLABSystem: '<S12>/Get Parameter3'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_h);



  // Terminate for MATLABSystem: '<S12>/Get Parameter4'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_mf);



  // Terminate for MATLABSystem: '<S12>/Get Parameter5'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_o);



  // Terminate for MATLABSystem: '<S2>/MATLAB System'

  matlabCodegenHandle_matlab_evqu(&cartesian_waypoints_planner_DW.obj);

  matlabCodegenHandle_matlabC_evq(&cartesian_waypoints_planner_DW.obj.IKInternal);

  emxFreeStruct_robotics_slmanip_(&cartesian_waypoints_planner_DW.obj);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_1);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_50);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_49);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_48);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_47);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_46);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_45);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_44);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_43);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_42);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_41);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_40);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_39);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_38);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_37);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_36);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_35);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_34);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_33);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_32);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_31);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_30);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_29);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_28);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_27);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_26);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_25);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_24);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_23);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_22);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_21);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_20);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_19);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_18);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_17);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_16);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_15);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_14);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_13);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_12);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_11);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_10);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_9);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_8);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_7);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_6);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_5);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_4);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_3);

  emxFreeStruct_c_rigidBodyJoint(&cartesian_waypoints_planner_DW.gobj_2);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_51);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_82);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_81);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_80);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_79);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_78);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_77);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_76);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_75);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_74);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_73);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_72);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_71);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_70);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_69);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_68);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_67);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_66);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_65);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_64);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_63);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_62);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_61);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_60);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_59);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_58);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_57);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_56);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_55);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_54);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_53);

  emxFreeStruct_w_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_52);

  emxFreeStruct_x_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_83);

  emxFreeStruct_x_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_84);

  emxFreeStruct_f_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_85);

  emxFreeStruct_h_robotics_core_i(&cartesian_waypoints_planner_DW.gobj_86);

  emxFreeStruct_h_robotics_core_i(&cartesian_waypoints_planner_DW.gobj_87);

  emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_88);

  emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_103);

  emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_102);

  emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_101);

  emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_100);

  emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_99);

  emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_98);

  emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_97);

  emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_96);

  emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_95);

  emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_94);

  emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_93);

  emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_92);

  emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_91);

  emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_90);

  emxFreeStruct_v_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_89);



  // Terminate for Atomic SubSystem: '<Root>/Subscribe1'

  // Terminate for MATLABSystem: '<S10>/SourceBlock'

  matlabCodegenHandle_mat_evqusng(&cartesian_waypoints_planner_DW.obj_pt);



  // End of Terminate for SubSystem: '<Root>/Subscribe1'



  // Terminate for MATLABSystem: '<Root>/Get Parameter'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_it);



  // Terminate for MATLABSystem: '<Root>/Get Parameter1'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_k);



  // Terminate for MATLABSystem: '<Root>/Get Parameter2'

  matlabCodegenHandle_matlabCodeg(&cartesian_waypoints_planner_DW.obj_fn);

  emxFreeStruct_robotics_slmani_e(&cartesian_waypoints_planner_DW.obj_m);

  emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_1_k);

  emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_16_d);

  emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_15_j);

  emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_14_l);

  emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_13_g);

  emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_12_j);

  emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_11_a);

  emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_10_p);

  emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_9_c);

  emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_8_p);

  emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_7_j);

  emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_6_j);

  emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_5_m);

  emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_4_i);

  emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_3_i);

  emxFreeStruct_n_robotics_manip_(&cartesian_waypoints_planner_DW.gobj_2_g);



  // Terminate for MATLABSystem: '<S13>/Set Parameter'

  cartesian_wa_matlabCodegenHa_lb(&cartesian_waypoints_planner_DW.obj_fb);



  // Terminate for MATLABSystem: '<S13>/Set Parameter1'

  cartesian_wa_matlabCodegenHa_lb(&cartesian_waypoints_planner_DW.obj_dr);



  // Terminate for MATLABSystem: '<S13>/Set Parameter2'

  cartesian_wa_matlabCodegenHa_lb(&cartesian_waypoints_planner_DW.obj_a5);



  // Terminate for MATLABSystem: '<S13>/Set Parameter3'

  cartesian_wa_matlabCodegenHa_lb(&cartesian_waypoints_planner_DW.obj_hu);



  // Terminate for MATLABSystem: '<S13>/Set Parameter4'

  cartesian_wa_matlabCodegenHa_lb(&cartesian_waypoints_planner_DW.obj_o1);



  // Terminate for MATLABSystem: '<S13>/Set Parameter5'

  cartesian_wa_matlabCodegenHa_lb(&cartesian_waypoints_planner_DW.obj_ap);



  // Terminate for Atomic SubSystem: '<S9>/Publish1'

  // Terminate for MATLABSystem: '<S17>/SinkBlock'

  matlabCodegenHandle_matla_evqus(&cartesian_waypoints_planner_DW.obj_h5);



  // End of Terminate for SubSystem: '<S9>/Publish1'



  // Terminate for Atomic SubSystem: '<Root>/Publish'

  // Terminate for MATLABSystem: '<S8>/SinkBlock'

  matlabCodegenHandle_matla_evqus(&cartesian_waypoints_planner_DW.obj_b);



  // End of Terminate for SubSystem: '<Root>/Publish'

}



//

// File trailer for generated code.

//

// [EOF]

//